#include "CTrackMatrix.h"
#include <iostream>
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/Vector.h"
#include <nlopt.hpp>
#include <nlopt.h>
#include <math.h>
#include "CVector3d.h"
#include<cmath>
#include<public.h>     //fileName
#include<fstream>     //fileName
#include<sstream>     //fileName
#include "f2c.h"
extern "C" {
	/* Subroutine */ int matin1_(doublereal *a, integer *dim1, integer *n1, 
			integer *n2, integer *indexx, integer *nerror, doublereal *determ);
	doublereal duni_0_(int n__, integer *iseed);
	doublereal duni_(void);
	doublereal dustar_(integer *iseed);
	doublereal formf_(doublereal *p);
	}
//attention: the input emittance is unnormalized rms
//the emittance used internal the code is unnormalized total
//the phase space internal code is zDelt in longitudinal direction,while in other direction is xxp
//the input unit is m*rad m/rad in all directions
//the twissVector function read the input emit and beami parameters and 
//    initinalize the hepmatrixSig
//the insig function turn the input longitudinal emittance to zDelt space 
//    and change the emittance\hepmatrixSig to zDelt space
using namespace std;
static double myvfunc(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data);
static double myvfunc2(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data);
static double myvfunc3(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data);
static double myvconstraint(const std::vector<double> &x, std::vector<double> &grad, void *data);
static double myvconstraint2(const std::vector<double> &x, std::vector<double> &grad, void *data);
static CTrackMatrix* s_trackMatrix;
typedef struct { int a; } my_constraint_data;
typedef struct { int a;double b; } my_constraint_data2;
static void *data;
static int iPeriod;
static int iCount=0;
static vector<CLHEP::HepSymMatrix>  vhdSig_Equip;
static int nv=6;
CTrackMatrix::CTrackMatrix():dh(1),flagE(1),dkx(0),dky(0),dkz(0),dk0x(0),dk0y(0),dk0z(0)
{
	hepmatrix=CLHEP::HepMatrix(6, 6,1);
	hepvector=CLHEP::HepVector(6, 0);
	hepvectorTwiss=CLHEP::HepVector(9,0);
	beami=CLHEP::HepVector(6,0);
	beamf=CLHEP::HepVector(6,0);
	beamo=CLHEP::HepVector(6,0);
	hepmatrixTwiss=CLHEP::HepMatrix(9,9,1);
	hepmatrixSig=CLHEP::HepSymMatrix(6,1);
	flagGapEmit=0;
}
CTrackMatrix::CTrackMatrix(double _dFreq,double _dEnergyIn,int _dh,int _flagE)
{
	iCell=0;
	dFreq=_dFreq*1.0e6;
	dh=_dh;
	dEnergyIn=_dEnergyIn;
	flagE=_flagE;
	dEnergyf=dEnergyIn;
	hepmatrix=CLHEP::HepMatrix(6, 6,1);
	hepvector=CLHEP::HepVector(6, 0);
	hepvectorTwiss=CLHEP::HepVector(9,0);
	beami=CLHEP::HepVector(6,0);
	beamf=CLHEP::HepVector(6,0);
	beamo=CLHEP::HepVector(6,0);
	hepmatrixTwiss=CLHEP::HepMatrix(9,9,1);
	flagGapEmit=0;
}
CTrackMatrix::~CTrackMatrix()
{}
void CTrackMatrix::inSig_modify(double dE)
//change the longitudinal parameters to zDelt space
{
  double dGamma;
	dGamma=dE/dErest+1.0;
	//beta(zDelt)=beta(zzp)/Gamma/Gamma
	//gamma(zDelt)=gamma(zzp)*Gamma*Gamma
	//emit(zDelt)=emit(zzp)*gamma*gamma
	//alph(zDelt)=alph(zzp)
	//beta(zDelt)*emit(zDelt)=beta(zzp)*emit(zzp)
	//gama(zDelt)*emit(zDelt)=(gamma^4)*gama(zzp)*emit(zzp)
	hepmatrixSig[5][5]*=(pow(dGamma,4));
	hepmatrixSig[5][4]*=(dGamma*dGamma);
	hepmatrixSig[4][5]*=(dGamma*dGamma);
	vhdSig[0]=hepmatrixSig;
	hepvectorTwiss[6]/=(dGamma*dGamma);
	hepvectorTwiss[8]*=(dGamma*dGamma);
	dEmitz*=(dGamma*dGamma);
	vvdEmit[0][2]=dEmitz;
}
void CTrackMatrix::setFreq(double _dFreq)
{ dFreq=_dFreq*1.0e6; }
void CTrackMatrix::setEnergyIn(double _dE)
{ dEnergyIn=_dE;}
void CTrackMatrix::BeamVector(vector<double>& vd)
{
	for(int i=0;i<6;i++)
		hepvector[i]=vd[i];
	//initialize sigma matrix
}
void CTrackMatrix::TwissVector(vector<double>& vd)
	//the input vector format is ax ay az bx by bz ex ey ez
	//and the hepvectorTwiss format is: bx ax gammax by ay gammay bz az gammaz
{
	double dtmp1,dtmp2,dtmp3;
	vhdSig.resize(0);
	dtmp1=(1.0+vd[0]*vd[0])/vd[3]; //gamax
	dtmp2=(1.0+vd[1]*vd[1])/vd[4]; //gamay
	dtmp3=(1.0+vd[2]*vd[2])/vd[5]; //gamaz
	hepvectorTwiss[0]=vd[3];   hepvectorTwiss[1]=vd[0];
	//hepvectorTwiss[2]=vd[6]*5; 
	hepvectorTwiss[3]=vd[4]; hepvectorTwiss[4]=vd[1]; 
	//hepvectorTwiss[5]=vd[7]*5;
	hepvectorTwiss[6]=vd[5]; hepvectorTwiss[7]=vd[2];
	//hepvectorTwiss[5]=vd[8]*5;
	dEmitx=vd[6]*5;dEmity=vd[7]*5;dEmitz=vd[8]*5;
	vector<double> vdTmp;
	vdTmp.push_back(dEmitx); vdTmp.push_back(dEmity); vdTmp.push_back(dEmitz);
	vvdEmit.resize(1);
	vvdEmit[0]=vdTmp;
//	vdTmp.resize(0);
//	vdTmp.push_back(vd[3]); vdTmp.push_back(vd[4]); vdTmp.push_back(vd[5]);
//	vvdBeta.push_back(vdTmp);
	hepvectorTwiss[2]=dtmp1; 
	hepvectorTwiss[5]=dtmp2;
	hepvectorTwiss[8]=dtmp3; 
	//initialize sigma matrix
	hepmatrixSig[0][0]= vd[3]*dEmitx; //beta*emit
	hepmatrixSig[0][1]=-vd[0]*dEmitx; //-alph*emit
	hepmatrixSig[1][1]= dtmp1*dEmitx; //gama*emit

	hepmatrixSig[2][2]= vd[4]*dEmity;
	hepmatrixSig[2][3]=-vd[1]*dEmity;
	hepmatrixSig[3][3]= dtmp2*dEmity;

	hepmatrixSig[4][4]= vd[5]*dEmitz;
	hepmatrixSig[4][5]=-vd[2]*dEmitz;
	hepmatrixSig[5][5]= dtmp3*dEmitz;

  vhdSig.push_back(hepmatrixSig);
}
CLHEP::HepMatrix & CTrackMatrix::Drift(double _dL,double _dEnergy)
{
	double dDLength=_dL/100.0/2.0;
	double dEnergy;
	if(_dEnergy==0) dEnergy=dEnergyIn;
	else            dEnergy=_dEnergy;
	double dGamma=1.0+dEnergy/dErest;
	CLHEP::HepMatrix *matrix=new CLHEP::HepMatrix(6,6,1);

	(*matrix)[0][1]=dDLength;
	(*matrix)[2][3]=dDLength;
	(*matrix)[4][5]=dDLength/dGamma/dGamma;
	hepmatrix=(*matrix);
	hepmatrixSig=hepmatrixSig.similarity(*matrix);
	//hepmatrixSig=hepmatrixSig.similarity(hepmatrix);
	//may be not necessary===
//	hepmatrixTwiss=TransTwiss(hepmatrix);
//	hepvectorTwiss=(hepmatrixTwiss)*(hepvectorTwiss);
//	vhdTwiss.push_back(hepvectorTwiss);
	//=======================
	//vhdSig.push_back(hepmatrixSig);
  *matrix=SC_linear(dDLength*2.0);
  hepmatrixSig=hepmatrixSig.similarity(*matrix);
  hepmatrix=(*matrix)*hepmatrix;

	delete matrix;
	matrix=new CLHEP::HepMatrix(6,6,1);
	(*matrix)[0][1]=dDLength;
	(*matrix)[2][3]=dDLength;
	(*matrix)[4][5]=dDLength/dGamma/dGamma;
	hepmatrix=(*matrix)*hepmatrix;
	hepmatrixSig=hepmatrixSig.similarity(*matrix);

	delete matrix;
	return hepmatrix;
}
CLHEP::HepMatrix & CTrackMatrix::Quad(double _dLQuad,double _dSQuad,double _dEnergy)
{
	double dLQuad=_dLQuad/100.0/2.0;
	double dEnergy,dBeta;
	if(_dEnergy==0) dEnergy=dEnergyIn;
	else            dEnergy=_dEnergy;
	double dGamma=1+dEnergy/dErest;
	if(_dLQuad==0) { hepmatrix=CLHEP::HepMatrix(6,6,1); }
	else if(_dSQuad==0) { hepmatrix=Drift(dLQuad*100,dEnergy); }
	else
	{
		dBeta=sqrt(1-1/dGamma/dGamma);
		CLHEP::HepMatrix *matrix=new CLHEP::HepMatrix(6,6,1);
		double dBr=dErest*dGamma*dBeta/clight*1.0e6;   //parameters haven't been defined,pay attention to e
		double dk=sqrt(fabs(_dSQuad/dBr));
		double d1,d2,d3,d4;
		int flag1=0;
		if(_dSQuad*chargeOverMass>0)
    {
      d1=cos(dk*dLQuad);d2=cosh(dk*dLQuad);d3=sin(dk*dLQuad);d4=sinh(dk*dLQuad);
      flag1=0;
    }
    else 
    {
      d1=cosh(dk*dLQuad);d2=cos(dk*dLQuad);d3=sinh(dk*dLQuad);d4=sin(dk*dLQuad);
      flag1=1;
    }
		(*matrix)[0][0]=(*matrix)[1][1]=d1;
		(*matrix)[2][2]=(*matrix)[3][3]=d2;
		(*matrix)[0][1]=d3/dk;
		(*matrix)[1][0]=dk*d3;
		(*matrix)[2][3]=d4/dk;
		(*matrix)[3][2]=dk*d4;
		(*matrix)[4][5]=dLQuad/dGamma/dGamma;
		if(flag1==0)    (*matrix)[1][0]*=-1;
		else if(flag1==1) (*matrix)[3][2]*=-1;
		hepmatrix=*matrix;
		hepmatrixSig=hepmatrixSig.similarity(*matrix);
		//cout<<"Quad\n"<<hepmatrix;
		*matrix=SC_linear(dLQuad*2.0);
		hepmatrix=(*matrix)*hepmatrix;
		hepmatrixSig=hepmatrixSig.similarity(*matrix);
    delete matrix;
    matrix=new CLHEP::HepMatrix(6,6,1);
		(*matrix)[0][0]=(*matrix)[1][1]=d1;
		(*matrix)[2][2]=(*matrix)[3][3]=d2;
		(*matrix)[0][1]=d3/dk;
		(*matrix)[1][0]=dk*d3;
		(*matrix)[2][3]=d4/dk;
		(*matrix)[3][2]=dk*d4;
		(*matrix)[4][5]=dLQuad/dGamma/dGamma;
		if(flag1==0)    (*matrix)[1][0]*=-1;
		else if(flag1==1) (*matrix)[3][2]*=-1;
		hepmatrix=(*matrix)*hepmatrix;
		hepmatrixSig=hepmatrixSig.similarity(*matrix);

		delete matrix;
	}
	//sig matrix and envelop transfer
	//vhdSig.push_back(hepmatrixSig);
	//may not be necessary============
//	hepmatrixTwiss=TransTwiss(hepmatrix);
//	hepvectorTwiss=(hepmatrixTwiss)*(hepvectorTwiss);
//	vhdTwiss.push_back(hepvectorTwiss);
	//=================================
  //cout<<"sig matrix after Quad\n"<<hepmatrixSig;
	return hepmatrix;
}
CLHEP::HepMatrix & CTrackMatrix::RFGap(double _dE0TL,double _dPhis,double _dEnergy)
{
	double dE0TLTmp=_dE0TL/100.0;   //unit MV
	double dEnergy,dEnergyAvg,dEnergyIncrease,dBetaIn,dBetaAvg,dBetaf,dGammaIn,dGammaf,dGammaAvg;
	dPhis=_dPhis*pi/180;
	if(_dEnergy==0)
		dEnergy=dEnergyIn;
	else
		dEnergy=_dEnergy;
	dGammaIn=dEnergy/dErest+1.0;dBetaIn=sqrt(1.0-1.0/dGammaIn/dGammaIn);
	dEnergyIncrease=dE0TLTmp*cos(dPhis);
	dEnergyAvg=dEnergy+dEnergyIncrease/2.0;dGammaAvg=dEnergyAvg/dErest+1.0;
	dBetaAvg=sqrt(1.0-1.0/dGammaAvg/dGammaAvg);
	dEnergyf=dEnergy+dEnergyIncrease;dGammaf=1+dEnergyf/dErest;dBetaf=sqrt(1-1/dGammaf/dGammaf);

	if(flagE!=0&&flagE!=1)
	{
		cout<<"flagE should equals 0 or 1";
		exit(2);
	}
	double dConst;
	dConst=pi*dh*dE0TLTmp*sin(dPhis)/dErest/dBetaAvg/dBetaAvg/dGammaAvg/dGammaAvg/clight*dFreq;
	CLHEP::HepMatrix *matrix=new CLHEP::HepMatrix(6,6,1);
	(*matrix)[1][1]=(*matrix)[3][3]=(*matrix)[5][5]=dBetaIn*dGammaIn/dBetaf/dGammaf;
	(*matrix)[1][0]=-(dConst/dBetaf/dGammaf);
	(*matrix)[3][2]=-(dConst/dBetaf/dGammaf);
	(*matrix)[5][4]=2*dConst/dBetaf/dGammaf*dGammaAvg*dGammaAvg;
	hepmatrix=*matrix;
	//sig matrix and envelop transfer
	hepmatrixSig=hepmatrixSig.similarity(hepmatrix);
//非归一化发射度变化
	//vhdSig.push_back(hepmatrixSig);
	//may not be necessary============
//	hepmatrixTwiss=TransTwiss(hepmatrix);
//	hepvectorTwiss=(hepmatrixTwiss)*(hepvectorTwiss);
//	vhdTwiss.push_back(hepvectorTwiss);
	//================================
	//cout<<"RFGap\n"<<(*matrix);
	//=========emittance increase in gap
	double emitIncre_Squre;
	double dXprms_Sq;
	double dP_W=sqrt(hepmatrixSig[4][4]); //Phase_With
	double df=15.0/dP_W/dP_W*(3.0/dP_W/dP_W*(sin(dP_W)/dP_W-cos(dP_W))-sin(dP_W)/dP_W);
	double df2=15.0/dP_W/dP_W/4.0*(3.0/dP_W/dP_W/4.0*(sin(dP_W*2.0)/dP_W/2.0-cos(2.0*dP_W))-sin(2.0*dP_W)/2.0/dP_W);
	double dg=0.5*(1-cos(2*dPhis)*df2);
	double eta=dBetaIn*dGammaIn/dBetaf/dGammaf;
	//dXprms_Sq=(dBetaIn*dGammaIn/dBetaf/dGammaf)*dxi-dConst*sin(dPhis)*df*dxi;
	dXprms_Sq=dConst*dConst/dBetaf/dGammaf/dBetaf/dGammaf*(dg-sin(dPhis)*sin(dPhis)*df*df)*hepmatrixSig[0][0];
	emitIncre_Squre=hepmatrixSig[0][0]*dXprms_Sq;
	dEmitx=sqrt(eta*eta*dEmitx*dEmitx+flagGapEmit*emitIncre_Squre);
	//y direction
	emitIncre_Squre=emitIncre_Squre/hepmatrixSig[0][0]/hepmatrixSig[0][0]*hepmatrixSig[2][2]*hepmatrixSig[2][2];
	dEmity=sqrt(eta*eta*dEmity*dEmity+flagGapEmit*emitIncre_Squre);
	//z direction
	dXprms_Sq=4*dConst*dConst*dGammaAvg*dGammaAvg*(cos(dPhis)*cos(dPhis)/8.0+sin(dPhis)*dP_W/576.0);
	dEmitz=dEmitz*pow(dGammaIn/dGammaf,3)*dBetaIn/dBetaf;
	//??
	//==================================
	//20130305 不需要
	//double tmp;
	//for(int i=0;i<hepmatrixSig.num_row();i++)
	//{
	//	tmp=hepmatrixSig[i][i];
	//	tmp/=eta;
	//	hepmatrixSig[i][i]=tmp;
	//}
	//tmp=hepmatrixSig[1][0];
	//tmp/=eta;
	//hepmatrixSig[1][0]=tmp;
	//tmp=hepmatrixSig[3][2];
	//tmp/=eta;
	//hepmatrixSig[3][2]=tmp;
	//tmp=hepmatrixSig[5][4];
	//tmp/=eta;
	//hepmatrixSig[5][4]=tmp;
	dEnergyIn=dEnergyf;
	delete matrix;
	return hepmatrix;
  //cout<<"sig matrix after Quad\n"<<hepmatrixSig;
}

void CTrackMatrix::DtlCell(double _dLeft,double _dRight,double _dE0TL,double _dPhis,
		double _dLQuad1,double _dSQuad1,double _dLQuad2,double _dSQuad2,double _dEnergy)//dLeft,dRight means the left and right cell length
{
	double dEnergy;
	double dL=(_dLeft+_dRight)/100.0;
	if(_dEnergy==0)
		dEnergy=dEnergyIn;
	else
		{ dEnergy=_dEnergy; dEnergyIn=dEnergy;}
//	vector<double> vdTmp;

	CLHEP::HepMatrix matrixCell(6,6,1);
	CLHEP::HepMatrix matrixElement(6,6,1);

//for(int i=0;i<10;i++)
//{
//	matrixElement=(Quad(_dLQuad1/10.0,_dSQuad1,0));
//	matrixCell=matrixElement*matrixCell;
//	}
	matrixCell=(Quad(_dLQuad1,_dSQuad1,0));
	matrixElement=Drift(_dLeft-_dLQuad1,0);
	matrixCell=(matrixElement)*(matrixCell);
	matrixElement=RFGap(_dE0TL,_dPhis,dEnergyIn);
	matrixCell=(matrixElement)*(matrixCell);
	matrixElement=Drift(_dRight-_dLQuad2,0);
	matrixCell=(matrixElement)*(matrixCell);
	matrixElement=Quad(_dLQuad2,_dSQuad2,0);
	matrixCell=(matrixElement)*(matrixCell);

	hepmatrix=matrixCell;
	//hepmatrixTwiss=TransTwiss(hepmatrix);

	// phase advance
	//vhdSig.push_back(hepmatrixSig);
	//dB0x=vhdSig[iCell][0][0]/dEmitx;
	//dB0y=vhdSig[iCell][2][2]/dEmity;
	//dB0z=vhdSig[iCell][4][4]/dEmitz;  //!!!!适合发射度没有变化的情况

	//dB1x=vhdSig[iCell+1][0][0]/dEmitx;
	//dB1y=vhdSig[iCell+1][2][2]/dEmity;
	//dB1z=vhdSig[iCell+1][4][4]/dEmitz;

  //dk0x=asin(hepmatrix[0][1]/sqrt(dB0x*dB1x));
  //dk0y=asin(hepmatrix[2][3]/sqrt(dB0y*dB1y));
  //dk0z=asin(hepmatrix[4][5]/sqrt(dB0z*dB1z));

	//vdTmp.resize(0);
	//vdTmp.push_back(iCell);
	//dk0x*=(180.0/pi);dk0y*=(180.0/pi);dk0z*=(180.0/pi);
	//vdTmp.push_back(dk0x);vdTmp.push_back(dk0y);vdTmp.push_back(dk0z);
	//vdTmp.push_back(dk0x/dL);vdTmp.push_back(dk0y/dL);vdTmp.push_back(dk0z/dL);
	//vvdPh_Av.push_back(vdTmp);
	//vhmatrix.push_back(hepmatrix);
}
vector<double> CTrackMatrix::CalSig2(const CLHEP::HepMatrix &hm)
{
   double det,dk, cosu;
	 vector<double>  vd;
	 for(int i=0;i<5;i=i+2)
	 {
		 det=hm[i][i]*hm[i+1][i+1]-hm[i][i+1]*hm[i+1][i];
		 cosu=(hm[i][i]+hm[i+1][i+1]/det)/2.0;
		 if(fabs(cosu)<=1.0)
			 dk=acos(cosu);
		 else dk=0.0;
		 vd.push_back(dk);
	 }
	 return vd;
}
vector<double> CTrackMatrix::CalSig3(vector<CLHEP::HepSymMatrix> &vhd,CLHEP::HepMatrix &vm,int i)
{
	double dB0x,dB0y,dB0z,dB1x,dB1y,dB1z;
	double dA0x,dA0y,dA0z,dA1x,dA1y,dA1z;
	double dE0x,dE0y,dE0z;
	double dE1x,dE1y,dE1z;
	double detx,dety,detz;
	double dE0,dE1;
	double dA0,dA1;
	double dB0,dB1;
	double det;
	double dk0;
	for(int k=0;k<5;k=k+2)
	{
		dE0=sqrt(vhd[i][k][k]*vhd[i][k+1][k+1]-vhd[i][k][k+1]*vhd[i][k+1][k]);
		dE1=sqrt(vhd[i+Period][k][k]*vhd[i+Period][k+1][k+1]-vhd[i+Period][k][k+1]*vhd[i+Period][k+1][k]);
		dB0= vhd[i][k][k]/dE0;
		dA0=-vhd[i][k][k+1]/dE0;
		dB1= vhd[i+Period][k][k]/dE1;
		dA1=-vhd[i+Period][k][k+1]/dE1;
		detx=vm[k][k]*vm[k+1][k+1]-vm[k][k+1]*vm[k+1][k];
		dk0=asin(hepmatrix[k][k+1]/sqrt(dB0*dB1*det));
		cout<<dk0<<endl;
		dk0=1.0/sqrt(det)*(vm[k][k]*sqrt(dB0/dB1)-vm[k+1][k+1]*sqrt(dB1/dB0))/(dA0+dA1);
		dk0=asin(dk0x);
		cout<<dk0<<endl;
		dk0=1.0/sqrt(det)*(vm[k][k]*sqrt(dB0/dB1)*dA1+dA0*hepmatrix[k+1][k+1]*sqrt(dB1/dB0))/(dA0+dA1);
		dk0=acos(dk0);
		cout<<dk0<<endl;
	}
  vector<double> vdTmp;
	vdTmp.resize(0);
	dk0x*=(180.0/pi);dk0y*=(180.0/pi);dk0z*=(180.0/pi);
	vdTmp.push_back(dk0x);vdTmp.push_back(dk0y);vdTmp.push_back(dk0z);

	return vdTmp;
}
vector<double> CTrackMatrix::CalSig(vector<CLHEP::HepSymMatrix> &vhd,int i)
{
	double dB0x,dB0y,dB0z,dB1x,dB1y,dB1z;
	double dA0x,dA0y,dA0z,dA1x,dA1y,dA1z;
	double dE0x,dE0y,dE0z;
	double dE1x,dE1y,dE1z;
	double detx,dety,detz;
	dE0x=sqrt(vhd[i][0][0]*vhd[i][1][1]-vhd[i][0][1]*vhd[i][1][0]);
	dE0y=sqrt(vhd[i][2][2]*vhd[i][3][3]-vhd[i][2][3]*vhd[i][3][2]);
	dE0z=sqrt(vhd[i][4][4]*vhd[i][5][5]-vhd[i][4][5]*vhd[i][5][4]);
	dE1x=sqrt(vhd[i+1][0][0]*vhd[i+1][1][1]-vhd[i+1][0][1]*vhd[i+1][1][0]);
	dE1y=sqrt(vhd[i+1][2][2]*vhd[i+1][3][3]-vhd[i+1][2][3]*vhd[i+1][3][2]);
	dE1z=sqrt(vhd[i+1][4][4]*vhd[i+1][5][5]-vhd[i+1][4][5]*vhd[i+1][5][4]);
	dB0x= vhd[i][0][0]/dE0x;
	dB0y= vhd[i][2][2]/dE0y;
	dB0z= vhd[i][4][4]/dE0z;  
	dA0x=-vhd[i][0][1]/dE0x;
	dA0y=-vhd[i][2][3]/dE0y;
	dA0z=-vhd[i][4][5]/dE0z;  

	dB1x= vhd[i+1][0][0]/dE1x;
	dB1y= vhd[i+1][2][2]/dE1y;
	dB1z= vhd[i+1][4][4]/dE1z;
	dA1x=-vhd[i+1][0][1]/dE1x;
	dA1y=-vhd[i+1][2][3]/dE1y;
	dA1z=-vhd[i+1][4][5]/dE1z;  

	detx=hepmatrix[0][0]*hepmatrix[1][1]-hepmatrix[0][1]*hepmatrix[1][0];
	dety=hepmatrix[2][2]*hepmatrix[3][3]-hepmatrix[2][3]*hepmatrix[3][2];
	detz=hepmatrix[4][4]*hepmatrix[5][5]-hepmatrix[4][5]*hepmatrix[5][4];
	cout<<"det is "<<detx<<" "<<dety<<" "<<detz<<endl;
	

  dk0x=asin(hepmatrix[0][1]/sqrt(dB0x*dB1x*detx));
  dk0y=asin(hepmatrix[2][3]/sqrt(dB0y*dB1y*dety));
  dk0z=asin(hepmatrix[4][5]/sqrt(dB0z*dB1z*detz));
	cout<<dk0x<<" "<<dk0y<<" "<<dk0z<<endl;

  dk0x=1.0/sqrt(detx)*(hepmatrix[0][0]*sqrt(dB0x/dB1x)-hepmatrix[1][1]*sqrt(dB1x/dB0x))/(dA0x+dA1x);
  dk0y=1.0/sqrt(dety)*(hepmatrix[2][2]*sqrt(dB0y/dB1y)-hepmatrix[3][3]*sqrt(dB1y/dB0y))/(dA0y+dA1y);
  dk0z=1.0/sqrt(detz)*(hepmatrix[4][4]*sqrt(dB0z/dB1z)-hepmatrix[5][5]*sqrt(dB1z/dB0z))/(dA0z+dA1z);
	dk0x=asin(dk0x);
	dk0y=asin(dk0y);
	dk0z=asin(dk0z);
	cout<<dk0x<<" "<<dk0y<<" "<<dk0z<<endl;

  dk0x=1.0/sqrt(detx)*(hepmatrix[0][0]*sqrt(dB0x/dB1x)*dA1x+dA0x*hepmatrix[1][1]*sqrt(dB1x/dB0x))/(dA0x+dA1x);
  dk0y=1.0/sqrt(dety)*(hepmatrix[2][2]*sqrt(dB0y/dB1y)*dA1y+dA0y*hepmatrix[3][3]*sqrt(dB1y/dB0y))/(dA0y+dA1y);
  dk0z=1.0/sqrt(detz)*(hepmatrix[4][4]*sqrt(dB0z/dB1z)*dA1z+dA0z*hepmatrix[5][5]*sqrt(dB1z/dB0z))/(dA0z+dA1z);
	dk0x=acos(dk0x);
	dk0y=acos(dk0y);
	dk0z=acos(dk0z);
	cout<<dk0x<<" "<<dk0y<<" "<<dk0z<<endl;

  vector<double> vdTmp;
	vdTmp.resize(0);
	dk0x*=(180.0/pi);dk0y*=(180.0/pi);dk0z*=(180.0/pi);
	vdTmp.push_back(dk0x);vdTmp.push_back(dk0y);vdTmp.push_back(dk0z);

	return vdTmp;
}
void CTrackMatrix::TransSig(CLHEP::HepMatrix &M)
{
	hepmatrixSig=hepmatrixSig.similarityT(hepmatrix);
  CLHEP::HepSymMatrix hsm=CLHEP::HepSymMatrix(2,1);
	hsm[0][0]=2;hsm[0][1]=3;hsm[1][1]=4;
	CLHEP::HepMatrix hm=CLHEP::HepMatrix(2,2,1);
	hm[0][1]=5;
	cout<<hsm.similarity(hm);
}
CLHEP::HepMatrix  CTrackMatrix::TransTwiss(CLHEP::HepMatrix &M)
	//已知传输矩阵求解相应的twiss参数（beta，alfa，gama）的传输矩阵.没考虑耦合
{
	//CLHEP::HepMatrix hepmatrixTwiss(9,9,0);
	//twiss(1)(1)=(M(1)(1))*(M(1)(1));twiss(1)(2)=(-2)*(M(1)(1))*(M(1)(2));
	hepmatrixTwiss[0][0]=M[0][0]*M[0][0];hepmatrixTwiss[0][1]=(-2)*M[0][0]*M[0][1];
	// twiss(1)(3)=(M(1)(2))*(M(1)(2));
	hepmatrixTwiss[0][2]=M[0][1]*M[0][1];
	hepmatrixTwiss[1][0]=(-1)*M[0][0]*M[1][0]; hepmatrixTwiss[1][1]=M[0][0]*M[1][1]+M[0][1]*M[1][0];
	//twiss(2)(1)=(-1)*M(1)(1)*M(1)(2);twiss(2)(2)=M(1)(1)*M(2)(2)+M(1)(2)*M(2)(1);
	hepmatrixTwiss[1][2]=(-1)*M[0][1]*M[1][1];
	//twiss(2)(3)=(-1)*M(1)(2)*M(2)(2);
	hepmatrixTwiss[2][0]=M[1][0]*M[1][0];hepmatrixTwiss[2][1]=(-2)*M[1][0]*M[1][1];hepmatrixTwiss[2][2]=M[1][1]*M[1][1];
	//twiss(3)(1)=M(2)(1)*M(2)(1);twiss(3)(2)=(-2)*M(2)(1)*M(2)(2);twiss(3)(3)=M(2)(2)*M(2)(2);
	hepmatrixTwiss[3][3]=M[2][2]*M[2][2];hepmatrixTwiss[3][4]=(-2)*M[2][2]*M[2][3];hepmatrixTwiss[3][5]=M[2][3]*M[2][3];
	//twiss(4)(4)=M(3)(3)*M(3)(3);twiss(4)(5)=(-2)*M(3)(3)*M(3)(4);twiss(4)(6)=M(3)(4)*M(3)(4);
	hepmatrixTwiss[4][3]=(-1)*M[2][2]*M[3][2];hepmatrixTwiss[4][4]=M[2][2]*M[3][3]+M[2][3]*M[3][2];
	//hepmatrixTwiss(5)(4)=(-1)*M(3)(3)*M(3)(4);twiss(5)(5)=M(3)(3)*M(4)(4)+M(3)(4)*M(4)(3);
	hepmatrixTwiss[4][5]=(-1)*M[2][3]*M[3][3];
	//twiss(5)(6)=(-1)*M(3)(4)*M(4)(4);
	hepmatrixTwiss[5][3]=M[3][2]*M[3][2];hepmatrixTwiss[5][4]=(-2)*M[3][2]*M[3][3];hepmatrixTwiss[5][5]=M[3][3]*M[3][3];
	//twiss(6)(4)=M(4)(3)*M(4)(3);twiss(6)(5)=(-2)*M(4)(3)*M(4)(4);twiss(6)(6)=M(4)(4)*M(4)(4);
	hepmatrixTwiss[6][6]=M[4][4]*M[4][4];hepmatrixTwiss[6][7]=(-2)*M[4][4]*M[4][5];hepmatrixTwiss[6][8]=M[4][5]*M[4][5];
	//twiss(7)(7)=M(5)(5)*M(5)(5);twiss(7)(8)=(-2)*M(5)(5)*M(5)(6);twiss(7)(9)=M(5)(6)*M(5)(6);
	hepmatrixTwiss[7][6]=(-1)*M[4][4]*M[5][4];hepmatrixTwiss[7][7]=M[4][4]*M[5][5]+M[4][5]*M[5][4];
	//twiss(8)(7)=(-1)*M(5)(5)*M(5)(6);twiss(8)(8)=M(5)(5)*M(6)(6)+M(5)(6)*M(6)(5);
	hepmatrixTwiss[7][8]=(-1)*M[4][5]*M[5][5];
	//twiss(8)(9)=(-1)*M(5)(6)*M(6)(6);
	hepmatrixTwiss[8][6]=M[5][4]*M[5][4];hepmatrixTwiss[8][7]=(-2)*M[5][4]*M[5][5];hepmatrixTwiss[8][8]=M[5][5]*M[5][5];
	// return twiss;
	return hepmatrixTwiss;
}
void CTrackMatrix::DtlTank_File(string filename)
{
	ifstream infile(filename.c_str());
	if(!infile)
	{
		cout<<"can't open structure file for matrix"<<endl;
		return;
	}
	vector<double> vd;
	vector<double> vdTmp;
	double dtmp;
	int itmp;
	string sRow;
	stringstream ss;
	getline(infile,sRow);
	ss.clear();
	ss<<sRow;
	ss>>itmp;
	setPeriod(itmp);
	double dL=0;
	CLHEP::HepMatrix  hp=CLHEP::HepMatrix(6,6,1);
	CLHEP::HepMatrix  hp2=CLHEP::HepMatrix(6,6,1);
	while(getline(infile,sRow))
	{
		if(sRow==""||sRow==";") continue;
		vd.resize(0);
		ss.clear();
		ss<<sRow;
		while(ss>>dtmp)
			vd.push_back(dtmp);
		vvdStruct.push_back(vd);
		iCell+=1;
	}
	vvdEmit.resize(iCell+1);
	for(int i=0;i<vvdStruct.size();i++)
	{
		vd=vvdStruct[i];
		//new add !to change the longitudinal parameters to zDelt space
		if(i==0)
			inSig_modify(vd[8]);
		DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		//the parameters in vd[i] i>9 is unnormalized total emittance
		//the longitudinal is in zDelt phase space 
		if(vd.size()>9)
		{ dEmitx=vd[9]; dEmity=vd[10]; dEmitz=vd[11]; }
		vdTmp.resize(3);
		vdTmp[0]=dEmitx; vdTmp[1]=dEmity; vdTmp[2]=dEmitz;
		vvdEmit[i+1]=vdTmp;

		vhdSig.push_back(hepmatrixSig);
		vdTmp=CalSig(vhdSig,i);     //计算一个单元的相移
		vvdPh_Av.push_back(vdTmp);
		vhmatrix.push_back(hepmatrix);
		//cal period 
		hp=hepmatrix*hp;
    if((i+1)%Period==0)
		{
		  vhmatrix_Period.push_back(hp);
			hp=CLHEP::HepMatrix(6,6,1);
		}
    //cal total matrix till the current cell
		int size=vhmatrix_Total.size();
		if(size==0)
			vhmatrix_Total.push_back(hepmatrix);
		else
		{
			hp2=hepmatrix*vhmatrix_Total[size-1];
			vhmatrix_Total.push_back(hp2);
		}
	}
	//period phase advance of matched beam and real beam 
  vector<double> vdPh_AdTmp;
	ofstream of("phase_advance_match");
	double dkxTmp,dkyTmp,dkzTmp;    //for matched beam
	double d1,d2,d3;
	double dGamma;
	of<<" kxReal kyreal  kzreal   kxMatch_I  kyMatch_I   "<<endl;
	double dDetx,dDety,dDetz;
	for(int i=0;i<vhmatrix_Period.size();i++)
	{
	  //match beam
		dDetx=vhmatrix_Period[i][0][0]*vhmatrix_Period[i][1][1]-vhmatrix_Period[i][1][0]*vhmatrix_Period[i][0][1];
		dDety=vhmatrix_Period[i][2][2]*vhmatrix_Period[i][3][3]-vhmatrix_Period[i][3][2]*vhmatrix_Period[i][2][3];
		dDetz=vhmatrix_Period[i][4][4]*vhmatrix_Period[i][5][5]-vhmatrix_Period[i][5][4]*vhmatrix_Period[i][4][5];
		dkxTmp=acos((vhmatrix_Period[i][0][0]+vhmatrix_Period[i][1][1]/dDetx)/2.0);
		dkyTmp=acos((vhmatrix_Period[i][2][2]+vhmatrix_Period[i][3][3]/dDety)/2.0);
		dkzTmp=acos((vhmatrix_Period[i][4][4]+vhmatrix_Period[i][5][5]/dDetz)/2.0);
		dkxTmp*=(180.0/pi);
		dkyTmp*=(180.0/pi);
		dkzTmp*=(180.0/pi);
		of<<i<<" "<<dkxTmp<<" "<<dkyTmp<<" "<<dkzTmp<<" ";
		//the normalized emittance is const; 
		dGamma=vvdStruct[0][8]/dErest+1.0;
		//gammaz(n)=gammaz(un)*beta*gamma*gamma*gamma
		//gammax(n)=gammax(un)*beta*gamma
		dkxTmp=sqrt(3.0*vvdEmit[0][3]*dGamma*dGamma/vvdEmit[0][1]/2.0-0.5);
		dkyTmp=sqrt(3.0*vvdEmit[0][3]*dGamma*dGamma/vvdEmit[0][2]/2.0-0.5);
		of<<dkxTmp<<" "<<dkyTmp<<endl;
		vdPh_AdTmp.resize(0);
    vdPh_AdTmp.push_back(dkxTmp);
    vdPh_AdTmp.push_back(dkyTmp);
    vvdPh_Av_Equip.push_back(vdPh_AdTmp);
	}
	of.close();
	of.open("matrix_Total");
	for(int i=0;i<vhmatrix_Total.size();i++)
		of<<vhmatrix_Total[i]<<endl;;
	of.close();
	of.open("matrix_Period");
	for(int i=0;i<vhmatrix_Period.size();i++)
		of<<vhmatrix_Period[i]<<endl;;
	of.close();
}
void  CTrackMatrix::getTwiss()
{
	ofstream of("Twiss_matrix");
	for(int i=0;i<vhdTwiss.size();i++)
		of<<vhdTwiss[i]<<endl;
	of.close();
}
void  CTrackMatrix::getPhaseAdvance()
{
	ofstream of("Phase_Advance");
	//每个cell的相移
	for(int i=0;i<vvdPh_Av.size();i++)
	{
	  of<<i<<" ";
		for(int j=0;j<vvdPh_Av[i].size();j++)
			of<<vvdPh_Av[i][j]<<" ";
		of<<endl;
	}
	of.close();
}
void  CTrackMatrix::getSigMatrix()
{
	ofstream of("Sig_Matrix");
	//每个元件的sig 矩阵
	for(int i=0;i<vhdSig.size();i++)
	{
		for(int j=0;j<vhdSig[i].num_row();j++)
		{
			of<<1000*sqrt(vhdSig[i][j][j])<<" ";  //in mm &mrad
			for(int k=0;k<j;k++)
					of<<vhdSig[i][j][k]/sqrt(vhdSig[i][j][j]*vhdSig[i][k][k])<<" ";   
			of<<endl;
		}
		of<<endl;
	}
	of.close();
	of.open("envelop_Matrix");
	double dB;
	for(int i=0;i<vhdSig.size()-1;i++)
	{
		of<<i<<" "<<1000*sqrt(vhdSig[i][0][0])<<" "<<1000*sqrt(vhdSig[i][2][2])<<" ";
		of<<sqrt(vhdSig[i][4][4])*1000<<" ";
    dB=vvdStruct[i][8]/dErest+1.0;
		dB=sqrt(1.0-1.0/dB/dB);
		dB=sqrt(vhdSig[i][4][4])/dB/clight*dFreq*360;
		of<<dB<<endl;
	}
	of.close();
}
CLHEP::HepMatrix CTrackMatrix::SC_linear(double dL)
{
  double dEx,dEy,dEz;
  double dRx,dRy,dRz;
  double dConst;
  double dGamma=dEnergyIn/dErest+1.0;
  double dBeta=sqrt(1.0-1.0/dGamma/dGamma);
	CLHEP::HepMatrix matrix=CLHEP::HepMatrix(6,6,1);
  dConst=1.0/4.0/pi/dPermit*3.0*dCurrent/dFreq;
  dRx=sqrt(hepmatrixSig[0][0]);
  dRy=sqrt(hepmatrixSig[2][2]);
  dRz=sqrt(hepmatrixSig[4][4]);
	double dp=dGamma*dRz/sqrt(dRx*dRy);
	doublereal dp1=dp;
	doublereal df1=formf_(&dp1);
	double df=df1;
  //double df=1.0/3.0/dp;
  dEx=dConst*(1.0-df)/dGamma/dGamma/dRx/(dRx+dRy)/dRz;
  dEy=dConst*(1.0-df)/dGamma/dGamma/dRy/(dRx+dRy)/dRz;
  dEz=dConst*df/dRx/dRy/dRz;
  matrix[1][0]=fabs(chargeOverMass)*dEx*dL/dErest/1.0e6/dBeta/dBeta/dGamma;
  matrix[3][2]=fabs(chargeOverMass)*dEy*dL/dErest/1.0e6/dBeta/dBeta/dGamma;
  matrix[5][4]=fabs(chargeOverMass)*dEz*dL/dErest/1.0e6/dBeta/dBeta/dGamma;
  return matrix;
}
void CTrackMatrix::setCurrent(double _dC)
{
  dCurrent=_dC/1000.0;
}
vector<double> CTrackMatrix::Equip_DesignQuad(int _iPeriod)
{
	vector<double> vd;
	double dkxT,dkyT,dkzT;
	double dEx,dEy,dEz;  //emittance
	double dGamma;
  vector<double> p_a_tmp;
	hepmatrixSig=vhdSig[_iPeriod*Period];
	CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1); // store the matrix multiply till the current cell
	for(int j=0;j<Period;j++)
	{
		vd=vvdStruct[_iPeriod*Period+j];
		DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		vm=hepmatrix*vm;
		//vhdSig[_iPeriod*Period+j+1]=hepmatrixSig; //no use
	}
	//unnormalizated emittance
	//p_a_tmp=CalSig2(vm);
	p_a_tmp=CalSig3(vhdSig,vm,_iPeriod*Period);
	dEx=vvdEmit[_iPeriod*Period][0];
	dEy=vvdEmit[_iPeriod*Period][1];
	dEz=vvdEmit[_iPeriod*Period][2];
	vm=CLHEP::HepMatrix(6,6,1); 
	dkzT=p_a_tmp[2];
	dkxT=dkzT*dEz/dEx;
	dkyT=dkzT*dEz/dEy;
	p_a_tmp[0]=dkxT;
	p_a_tmp[1]=dkyT;
	//the follwing function change the quad strength to design
	return p_a_tmp;
}
void CTrackMatrix::Lattice_MMF(string str)
{
  ofstream of(str.c_str());
  int Per_Num=vvdStruct.size()/Period;
	vector<double> FF;//mismatch function;
	vector<double> vdMisMatch;//mismatch factor;
	for(int i=0;i<beami.num_row();i++)
	{
		int k=i+1;
		if(i<2) k=i;
		else if(i>3) k=i+2;
		beami[i]=hepvectorTwiss[k];
	}
	for(int i=0;i<Per_Num-1;i++)
	{                
		Match_Follow(vvdStruct,i*Period);
		Match_MMF(vdMisMatch,FF);
		of<<i<<" "<<vdMisMatch[0]<<" "<<vdMisMatch[1]<<" "<<vdMisMatch[2]<<endl;
		beami=beamo;
	}
	of.close();
}
void CTrackMatrix::InitLattice()
{
  Lattice_MMF("orign-mmf");
	double dITmp=dCurrent;
	dCurrent=0;
  vector<double> p_a_tmp;
  int Per_Num=vvdStruct.size()/Period;
	double dkxT,dkyT,dkzT;
	double dEx,dEy,dEz;  //emittance
	double dGamma;
	CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1); // store the matrix multiply till the current cell
	vector<double> vd;
	ofstream of("Equip_ph_ad_0");   
	ofstream of2("vvdStruct_Equip0");   
	of<<"#period "<<"dkx-orign"<<" "<<"dky-orign "<<"dkx-equip "<<"dky-equip"<<endl;
	double dQuad_tmp=0;
	for(iPeriod=0;iPeriod<Per_Num;iPeriod++)
	{
		hepmatrixSig=vhdSig[iPeriod*Period];
		for(int j=0;j<Period;j++)
		{
			vd=vvdStruct[iPeriod*Period+j];
			DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
			//cal period 
			vm=hepmatrix*vm;
		}
	  dGamma=vvdStruct[iPeriod*Period][8]/dErest+1.0;
		//unnormalizated emittance
		//p_a_tmp=CalSig2(vm);
		p_a_tmp=CalSig3(vhdSig,vm,iPeriod*Period);
	  dEx=vvdEmit[iPeriod*Period][0];
	  dEy=vvdEmit[iPeriod*Period][1];
	  dEz=vvdEmit[iPeriod*Period][2];
		vm=CLHEP::HepMatrix(6,6,1); 
		dkzT=p_a_tmp[2];
		of<<iPeriod<<" "<<p_a_tmp[0]*180.0/pi<<" "<<p_a_tmp[1]*180.0/pi<<" ";
		//emit(zz',n)=emit(zDelt)*beta*gamma
	  dkxT=sqrt(3.0*dEz/dEx/2.0-0.5)*dkzT;
	  dkyT=sqrt(3.0*dEz/dEy/2.0-0.5)*dkzT;
		p_a_tmp[0]=dkxT;
		p_a_tmp[1]=dkyT;
		vvdph_ad_0.push_back(p_a_tmp);
		Equip_Cell(dkxT,dkyT,1);
		hepmatrixSig=vhdSig[iPeriod*Period];
		for(int j=0;j<Period;j++)
		{
			vd=vvdStruct[iPeriod*Period+j];
			DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
			vm=hepmatrix*vm;
			for(int k=0;k<vd.size();k++)
			  of2<<vd[k]<<" ";
			of2<<endl;
		}
		//p_a_tmp=CalSig2(vm);
		p_a_tmp=CalSig3(vhdSig,vm,iPeriod*Period);
		vm=CLHEP::HepMatrix(6,6,1); 
		cout<<"the matched p_a is :"<<p_a_tmp[0]*180/pi<<" "<<p_a_tmp[1]*180/pi<<"\n"
		    <<"the design  p_a is :"<<dkxT*180/pi<<" "<<dkyT*180/pi<<endl;
		of<<dkxT*180/pi<<" "<<dkyT*180/pi<<endl;
	}
	of.close();
	of2.close();
	dCurrent=dITmp;
  Lattice_MMF("Equip0-mmf");
}
void CTrackMatrix::Equip_Tradtion()
{
/* used to simulate the tradition equip process
 * include the following process:
 * generate the init lattice according to the 0current ph_ad from smooth theory
 * match the lattice  (30mA)
 * transfer the beam through and get the ph_ad,envelop,mmf
 */
	double dFc=1.0;
	string   cNextStep;
	double dGammax,dGammay,dGammaz;
	//init lattice
  InitLattice();
	hepmatrix=vhdSig[0];
	//match
	for(int i=0;i<beami.num_row();i++)
	{
		int k=i+1;
		if(i<2) k=i;
		else if(i>3) k=i+2;
		beami[i]=hepvectorTwiss[k];
	}
	while(dFc>0.01)
	{ //还要再完善，考虑是计算失败还是只是没有达到精度
		dFc=Match(30,0);
		cout<<"press words: \n c-the convergency factor is right but not small enough, continue"<<endl;
		cout<<"r-the convergency is not right,restart the match process"<<endl;
		cout<<"o-OK"<<endl;
		cin>>cNextStep;
		if(cNextStep=="c"||cNextStep=="o")
		{
			for(int i=0;i<nv;i++)
			{
				int k=i+1;
				if(i<2) k=i;
				else if(i>3) k=i+2;
				hepvectorTwiss[k]=beami[i];
			}
			//				vhdSig.resize(1);
			dGammax=(1.0+beami[1]*beami[1])/beami[0];
			dGammay=(1.0+beami[3]*beami[3])/beami[2];
			dGammaz=(1.0+beami[5]*beami[5])/beami[4];
			hepvectorTwiss[2]=dGammax;
			hepvectorTwiss[5]=dGammay;
			hepvectorTwiss[8]=dGammaz;
			hepmatrixSig=vhdSig[0];
			hepmatrixSig[0][0]=beami[0]*vvdEmit[0][0];
			hepmatrixSig[2][2]=beami[2]*vvdEmit[0][1];
			hepmatrixSig[4][4]=beami[4]*vvdEmit[0][2];
			hepmatrixSig[1][0]=-beami[1]*vvdEmit[0][0];
			hepmatrixSig[3][2]=-beami[3]*vvdEmit[0][1];
			hepmatrixSig[5][4]=-beami[5]*vvdEmit[0][2];
			hepmatrixSig[1][1]=dGammax*vvdEmit[0][0];
			hepmatrixSig[3][3]=dGammay*vvdEmit[0][1];
			hepmatrixSig[5][5]=dGammaz*vvdEmit[0][2];
			vhdSig[0]=hepmatrixSig;
		}
		else if(cNextStep=="r") continue;
	}
	//ph_adv
	hepmatrixSig=vhdSig[0];
	CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1);
	vector<double> vd;
	ofstream of("ph_ad_Equip_trad");
	vector<double> p_a_tmp;
	for(int i=0;i<vvdStruct.size();i++)
	{
	  vd=vvdStruct[i];
		DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		vhdSig[i+1]=hepmatrixSig;
		vm=hepmatrix*vm;
		if((i+1)%4==0)
		{
			//p_a_tmp=CalSig2(vm);
			p_a_tmp=CalSig3(vhdSig,vm,iPeriod*Period);
			vm=CLHEP::HepMatrix(6,6,1);
			of<<(i+1)/4<<" "<<p_a_tmp[0]<<" "<<p_a_tmp[1]<<" "<<p_a_tmp[2]<<" ";
			of<<vvdEmit[0][0]<<" "<<vvdEmit[0][1]<<" "<<vvdEmit[0][2]<<endl;
		}
	}
	of.close();
	//mismatch factor
	Lattice_MMF("equip_trad_mmf");
}
void CTrackMatrix::Equip_Match()
{
  nv=6;
  int Per_Num=vvdStruct.size()/Period;
	int nit=1000;
	double dkxOld,dkyOld;
	vector<double> vdk;
	double dDiffx,dDiffy;
  //InitLattice();
	//为了避免调试过程中重复进行这一步，把init的结果存贮在文件，这里直接从文件读取
	ifstream infile("vvdStruct_Equip2");
	double x1,x2;  
	vector<double> vd_tmp_initLattice;
	stringstream ss;
	string sRow;
	if(!infile)
	{
		cout<<"can't open the vvdStruct_Equip_0 file"<<endl;
		return;
	}
	int i=0;
	//change back to include the initLattice effect
	//while(!infile.eof())
	//{
	//	getline(infile,sRow);
	//	if(sRow=="") continue;
	//	ss.clear();
	//	ss.str(sRow);        
	//	vd_tmp_initLattice.resize(0);
	//	while(ss>>x1)
	//		vd_tmp_initLattice.push_back(x1);
	//	vvdStruct[i]=vd_tmp_initLattice;
	//	i++;
	//}
	//Period=8;
	//end file reading for init lattice
//	dkxOld=vvdph_ad_0[0][0];
//	dkyOld=vvdph_ad_0[0][1];
	dkxOld=90.8372;
	dkyOld=90.8372;
	int xFlag=0,yFlag=0;
	double dFc=1;
	// the temination criter is Equip
	//char  cNextStep;
	string  cNextStep;
	double  dGammax,dGammay,dGammaz;
	vector<double> vdMisMatch;
	vector<double> FF;
	while(1)
	{
		//get the matched beam
		while(dFc>0.01)
		{ //还要再完善，考虑是计算失败还是只是没有达到精度
			dFc=Match(30,0);
			cout<<"press words: \n c-the convergency factor is right but not small enough, continue"<<endl;
			cout<<"r-the convergency is not right,restart the match process"<<endl;
			cout<<"o-OK"<<endl;
      cin>>cNextStep;
			if(cNextStep=="c"||cNextStep=="o")
			{
				for(int i=0;i<nv;i++)
				{
					int k=i+1;
					if(i<2) k=i;
					else if(i>3) k=i+2;
					hepvectorTwiss[k]=beami[i];
				}
//				vhdSig.resize(1);
				dGammax=(1.0+beami[1]*beami[1])/beami[0];
				dGammay=(1.0+beami[3]*beami[3])/beami[2];
				dGammaz=(1.0+beami[5]*beami[5])/beami[4];
				hepvectorTwiss[2]=dGammax;
				hepvectorTwiss[5]=dGammay;
				hepvectorTwiss[8]=dGammaz;
				hepmatrixSig=vhdSig[0];
				hepmatrixSig[0][0]=beami[0]*vvdEmit[0][0];
				hepmatrixSig[2][2]=beami[2]*vvdEmit[0][1];
				hepmatrixSig[4][4]=beami[4]*vvdEmit[0][2];
				hepmatrixSig[1][0]=-beami[1]*vvdEmit[0][0];
				hepmatrixSig[3][2]=-beami[3]*vvdEmit[0][1];
				hepmatrixSig[5][4]=-beami[5]*vvdEmit[0][2];
				hepmatrixSig[1][1]=dGammax*vvdEmit[0][0];
				hepmatrixSig[3][3]=dGammay*vvdEmit[0][1];
				hepmatrixSig[5][5]=dGammaz*vvdEmit[0][2];
				vhdSig[0]=hepmatrixSig;
			}
			else if(cNextStep=="r") continue;
			//else if(cNextStep=="o") break;
		}
		hepmatrixSig=vhdSig[0];
		vdk=Equip_DesignQuad(0);
		dDiffx=vdk[0]*180.0/pi-dkxOld;
		dDiffy=vdk[1]*180.0/pi-dkyOld;
		if(fabs(dDiffy)>0.1||fabs(dDiffx)>0.1)
		{
			dkxOld=vdk[0]*180.0/pi; dkyOld=vdk[1]*180.0/pi;
			//worried about the nonlinear programming can't mantain the equality of the other direction
			//if(dDiffy<0.1) yFlag=0;
			//if(dDiffx<0.1) xFlag=0;
			cout<<vdk[0]<<" "<<vdk[1]<<endl;
			//
			cout<<"the oldest quad :"<<vvdStruct[0][7]<<" "<<vvdStruct[1][7]<<" "
				  <<vvdStruct[2][7]<<" "<<vvdStruct[3][7]<<endl;

			hepmatrixSig=vhdSig[iPeriod*Period];
			xFlag=1;
			Equip_Cell(vdk[0],vdk[1],xFlag,yFlag);/*update the hepmatrix and vhdSig
			
			 for the following period*/
			hepmatrixSig=vhdSig[iPeriod*Period];
			Match_Follow(vvdStruct,iPeriod*Period);
			dFc=Match_MMF(vdMisMatch,FF);
			hepmatrixSig=vhdSig[iPeriod*Period];
			xFlag=0;

			cout<<"the newest quad :"<<vvdStruct[0][7]<<" "<<vvdStruct[1][7]<<" "
				  <<vvdStruct[2][7]<<" "<<vvdStruct[3][7]<<endl;
			cout<<dFc<<endl;
		}
		else break;
	}
	cout<<"the newest phase_adv "<<vdk[0]<<" "<<vdk[1]<<endl;
	cout<<"the newest twiss :";
	for(int i=0;i<beami.num_row();i++)
		cout<<beami[i]<<" ";
	cout<<endl;
	cout<<"the newest quad :"<<vvdStruct[0][7]<<" "<<vvdStruct[1][7]<<" "
	    <<vvdStruct[2][7]<<" "<<vvdStruct[3][7]<<endl;
	//用hepmatrix更新hepmatrixig
  infile.close();
	ofstream of("Equip_ph_ad_1");
	ofstream of2("beamIOF");
	of<<"#Period "<<" kx "<<" ky "<<" kz "<<" mmfx "<<"mmfy "<<" mmfz "<<" emitx "<<"emity "<<" emitz "<<endl;
	int flag=1;
	vector<CLHEP::HepVector>  vhv_beami; //store the beami in every Period entrance
	vhv_beami.resize(Per_Num+1);
	int iPeriod_bad;
	vhv_beami[0]=beami;
	double dP_A=0;
	while(flag)
	{
	  //beami变为初始
		for(iPeriod=0;(iPeriod+1)*Period<vvdStruct.size();iPeriod++)
		{
			beami=vhv_beami[iPeriod];
			hepmatrixSig=vhdSig[iPeriod*Period];
			vdk=Equip_DesignQuad(iPeriod);

// new add match section
      int new_match_flag=0;
			if(((vdk[2]-dP_A)>0.3||vdk[2]<5.0/180.0*pi)&&iPeriod>0&&(iPeriod+1)*Period<vvdStruct.size())
			{
			  new_match_flag=1;
			  cout<<"new match "<<vdk[2]<<endl<<endl;
			  dFc=1;
				while(dFc>0.1)
				{
					dFc=Match(40,iPeriod*Period);
					cout<<"input a char for nextstep "<<iPeriod<<endl;
					cin>>cNextStep;
					if(cNextStep=="r") continue;
					//更新sig matrix 和 beami
					dGammax=(1.0+beami[1]*beami[1])/beami[0];
					dGammay=(1.0+beami[3]*beami[3])/beami[2];
					dGammaz=(1.0+beami[5]*beami[5])/beami[4];
					hepmatrixSig=vhdSig[iPeriod*Period];
					dEmitx=sqrt(hepmatrixSig[0][0]*hepmatrixSig[1][1]-pow(hepmatrixSig[1][0],2));
					dEmity=sqrt(hepmatrixSig[2][2]*hepmatrixSig[3][3]-pow(hepmatrixSig[3][2],2));
					dEmitz=sqrt(hepmatrixSig[4][4]*hepmatrixSig[5][5]-pow(hepmatrixSig[5][4],2));
					hepmatrixSig[0][0]= beami[0]*dEmitx;
					hepmatrixSig[2][2]= beami[2]*dEmity;
					hepmatrixSig[4][4]= beami[4]*dEmitz;
					hepmatrixSig[1][0]=-beami[1]*dEmitx;
					hepmatrixSig[3][2]=-beami[3]*dEmity;
					hepmatrixSig[5][4]=-beami[5]*dEmitz;
					hepmatrixSig[1][1]=  dGammax*dEmitx;
					hepmatrixSig[3][3]=  dGammay*dEmity;
					hepmatrixSig[5][5]=  dGammaz*dEmitz;
					vhdSig[iPeriod*Period]=hepmatrixSig;
					vdk=Equip_DesignQuad(iPeriod);
					dkxOld=0;dkyOld=0;
					dDiffx=vdk[0]*180.0/pi-dkxOld;
					dDiffy=vdk[1]*180.0/pi-dkyOld;
					if(fabs(dDiffy)>0.1||fabs(dDiffx)>0.1)
					{
						dkxOld=vdk[0]*180.0/pi; dkyOld=vdk[1]*180.0/pi;
						cout<<vdk[0]<<" "<<vdk[1]<<endl;
						Equip_Cell(vdk[0],vdk[1],xFlag,yFlag);
						hepmatrixSig=vhdSig[iPeriod*Period];
						Match_Follow(vvdStruct,iPeriod*Period);
						dFc=Match_MMF(vdMisMatch,FF);
					}
				}
// add the fit section
				iPeriod=iPeriod-1;
				hepmatrixSig=vhdSig[iPeriod*Period];
				beamf=beami;
				beami=vhv_beami[iPeriod];
				nv=3;
				Match8_1(1000,iPeriod*Period,4);
				nv=6;
				//mismatch factor,mt=8
				hepmatrixSig=vhdSig[iPeriod*Period];
				dFc=Match_MMF(vdMisMatch,FF,8);
				//cal the ph_ad
				hepmatrixSig=vhdSig[iPeriod*Period];
				CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1); // store the matrix multiply till the current cell
				vector<double> vd;
				for(int j=0;j<Period;j++)
				{
					vd=vvdStruct[iPeriod*Period+j];
					DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
					vm=hepmatrix*vm;
					vhdSig[iPeriod*Period+j+1]=hepmatrixSig;
				}
				//vdk=CalSig2(vm);
				vdk=CalSig3(vhdSig,vm,iPeriod*Period);
				//store the phase advance,mismatch factor beamf beami and beamo
				of<<iPeriod<<" "<<vdk[0]<<" "<<vdk[1]<<" "<<vdk[2]<<" ";
				of<<vdMisMatch[0]<<" "<<vdMisMatch[1]<<" "<<vdMisMatch[2]<<" "<<new_match_flag<<endl;
				of2<<iPeriod<<endl;;
				for(int i=0;i<beami.num_row();i++)
					of2<<beami[i]<<" ";
				of2<<endl;
				for(int i=0;i<beami.num_row();i++)
					of2<<beamo[i]<<" ";
				of2<<endl;
				for(int i=0;i<beami.num_row();i++)
					of2<<beamf[i]<<" ";
				of2<<endl;
				beami=beamo;
				vhv_beami[iPeriod+1]=beami;
				continue;
			}
//			hepmatrixSig=vhdSig[(iPeriod-1)*Period];
// end new add section 
// end the fit section

			/*
			if(vdk[2]<dZero)
			{
				iPeriod=iPeriod-3;
				if(iPeriod_bad==iPeriod&&xFlag<=100)
				{
					xFlag+=1;
					iPeriod_bad=iPeriod;
				}
				else if(iPeriod_bad==iPeriod&&xFlag>100&&yFlag<=200)
				{
				  if(xFlag==101)
					{
						xFlag=102;
						yFlag=100;
					}
					yFlag+=1;
				}
				else if(iPeriod_bad==iPeriod&&xFlag>100&&yFlag>200)
				{
				  cout<<"error in finding the equip result"<<endl;
					cout<<" should input 0 for flag to stop the code"<<endl;
					break;
				}
				else if(iPeriod!=iPeriod_bad)
				{
				  iPeriod_bad=iPeriod;
					xFlag=0;
					yFlag=0;
				}
				continue;
			}
			*/
			Equip_Cell(vdk[0],vdk[1],xFlag,yFlag);
			of<<iPeriod<<" "<<vdk[0]<<" "<<vdk[1]<<" "<<vdk[2]<<" ";
			dP_A=vdk[2];
			hepmatrixSig=vhdSig[iPeriod*Period];
			if((iPeriod+1)*Period<vvdStruct.size()-1)
			{
				Match_Follow(vvdStruct,iPeriod*Period);
				dFc=Match_MMF(vdMisMatch,FF);
				of2<<iPeriod<<endl;;
				for(int i=0;i<beami.num_row();i++)
					of2<<beami[i]<<" ";
				of2<<endl;
				for(int i=0;i<beami.num_row();i++)
					of2<<beamo[i]<<" ";
				of2<<endl;
				for(int i=0;i<beami.num_row();i++)
					of2<<beamf[i]<<" ";
				of2<<endl;
				beami=beamo;
				vhv_beami[iPeriod+1]=beami;
			}
			else
				for(int i=0;i<vdMisMatch.size();i++)
					vdMisMatch[i]=0;
			of<<vdMisMatch[0]<<" "<<vdMisMatch[1]<<" "<<vdMisMatch[2]<<" "<<new_match_flag<<" ";
			//cal emit
			//double dEmit;
			//for(int i=0;i<5;i=i+2)
			//{
			//	dEmit=sqrt(vhdSig[iPeriod*Period][i][i]*vhdSig[iPeriod*Period][i+1][i+1]-pow(vhdSig[iPeriod*Period][i][i+1],2));
			//	of<<dEmit<<" ";
			//}
			//of<<endl;
			of<<vvdEmit[iPeriod*Period][0]<<" "<<vvdEmit[iPeriod*Period][1]<<" "<<vvdEmit[iPeriod*Period][2]<<endl;
			//of<<vdMisMatch[0]<<" "<<vdMisMatch[1]<<" "<<vdMisMatch[2]<<" "<<endl;
		}
		cout<<"input the flag:0 end;1 continue"<<endl;
		cin>>flag;
	}
	of.close();
	of2.close();
	of.open("vvdStruct_Equip2");
	for(int i=0;i<vvdStruct.size();i++)
	{
		for(int j=0;j<vvdStruct[i].size();j++)
		 of<<vvdStruct[i][j]<<" ";;
	  of<<endl;
	}
	of.close();                          
	//track the new structure in 0 current
	//the phase advance of the new structure seems right, especially the longitudianal phase advance 
	// is smooth
	double dCurrent_tmp;
	dCurrent_tmp=dCurrent;
	dCurrent=0;
	beami=vhv_beami[0];
	hepmatrixSig=vhdSig[0];
	CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1); // store the matrix multiply till the current cell
	of.open("last_stru_0_Curr");
	vector<double> vd,p_a_tmp;
	for(int _iPeriod=0;_iPeriod<Per_Num;_iPeriod++)
	{
		vm=CLHEP::HepMatrix(6,6,1); 
		for(int j=0;j<Period;j++)
		{
			vd=vvdStruct[_iPeriod*Period+j];
			DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
			vm=hepmatrix*vm;
		}
		//unnormalizated emittance
		//p_a_tmp=CalSig2(vm);
		p_a_tmp=CalSig3(vhdSig,vm,iPeriod*Period);
		of<<_iPeriod<<" "<<p_a_tmp[0]<<" "<<p_a_tmp[1]<<" "<<p_a_tmp[2]<<endl;
	}
	dCurrent=dCurrent_tmp;
	of.close();
}
int CTrackMatrix::Match_Conver(vector<vector<double > > &vvd)
{
    vector<double> vdTmp;
		double d_max;
		int    i_max;
		double det=1.0;
		vector<int> vi;
		vi.resize(vvd.size()); //index
		int Error=0;
		//改造矩阵
		vdTmp.resize(0);
		for(int i=0;i<vvd[0].size();i++)
		{
		  if(i==0)
			vdTmp.push_back(1);
			else
			vdTmp.push_back(0);
		}
		vvd[nv+1]=vdTmp;
//		cout<<"vvd is "<<endl;
//		for(int l=0;l<vvd.size();l++)
//		{
//			for(int j=0;j<vvd[l].size();j++)
//				cout<<vvd[l][j]<<" ";
//			cout<<endl;
//		}
//		cout<<endl;
		//vvd.push_back(vdTmp);
		//开始寻找
		for(int i=0;i<vvd.size();i++)
		{
			//输出
			//for(int l=0;l<vvd.size();l++)
			//{
			//	for(int j=0;j<vvd[l].size();j++)
			//		cout<<vvd[l][j]<<" ";
			//	cout<<endl;
			//}
			//cout<<endl;
		  d_max=vvd[i][i]; i_max=i;
			//如果某一行为0，error＝1，停止，否则吧该列调整到第i列
			for(int j=i+1;j<vvd[i].size();j++)
			{
				if(fabs(d_max<vvd[i][j]))  {d_max=vvd[i][j];i_max=j;}
			}
			if(fabs(d_max)<dZero) 
			{
			Error=1;
			
			return Error;;}
			vi[i]=i_max;
			if(i_max!=i)
			{
				//change row i and row i_max
				det=-det;
				double dtmp;
				for(int j=0;j<vvd.size();j++)
				{
					dtmp=vvd[j][i_max];
					vvd[j][i_max]=vvd[j][i];
					vvd[j][i]=dtmp;
				}
			}
			det=det*vvd[i][i];
			if(i!=vvd.size()-1)
			{
				for(int j=i+1;j<vvd[i].size();j++)
				{
					vvd[i][j]/=vvd[i][i];
				}
			}
			vvd[i][i]/=vvd[i][i];
			for(int j=i+1;j<vvd.size();j++)
			{
				for(int k=i+1;k<vvd[j].size();k++)
					vvd[j][k]=vvd[j][k]-vvd[j][i]*vvd[i][k]/vvd[i][i];
				vvd[j][i]=0;
			}
		}
//		for(int i=0;i<vvd.size();i++)
//		{
//			for(int j=0;j<vvd[i].size();j++)
//				cout<<vvd[i][j]<<" ";
//			cout<<endl;
//		}
		int j=0;
		for(int i=vi.size()-1;i=0;i--)
		{
			j=vi[i];
			if(i!=j)
				vdTmp=vvd[i];
			vvd[i]=vvd[j];
			vvd[j]=vdTmp;
		}
		return Error;
}
double CTrackMatrix::Match_MMF(vector<double>& vd2,vector<double> &FF,int mt)
{
   double dR,dM,dMax;
//	 vector<double>  FF;
	 FF.resize(6);
	 if(mt==8)  FF.resize(4);
	 double FF_Tmp;
	 double dGx,dGy,dGz,dgx,dgy,dgz;

   vd2.resize(0);
   dGx=(1.0+beamf[1]*beamf[1])/beamf[0];
   dGy=(1.0+beamf[3]*beamf[3])/beamf[2];
   dGz=(1.0+beamf[5]*beamf[5])/beamf[4];
   dgx=(1.0+beamo[1]*beamo[1])/beamo[0];
   dgy=(1.0+beamo[3]*beamo[3])/beamo[2];
   dgz=(1.0+beamo[5]*beamo[5])/beamo[4];
	 dR=dGx*beamo[0]+dgx*beamf[0]-2*beamf[1]*beamo[1];
	 dMax=sqrt(0.5*(dR+sqrt(fabs(dR*dR-4.0))))-1.0;
	 vd2.push_back(dMax);
	 dR=dGy*beamo[2]+dgy*beamf[2]-2*beamf[3]*beamo[3];
	 dM=sqrt(0.5*(dR+sqrt(fabs(dR*dR-4.0))))-1.0;
	 if(dM>dMax) dMax=dM;
	 vd2.push_back(dM);
	 if(mt!=8)
	 {
		 dR=dGz*beamo[4]+dgz*beamf[4]-2*beamf[5]*beamo[5];
		 dM=sqrt(0.5*(dR+sqrt(fabs(dR*dR-4.0))))-1.0;
	 }
	 else if(mt==8) dM=0;
	 if(dM>dMax) dMax=dM;
	 vd2.push_back(dM);
	 cout<<"mismatch factor is  ";
	 for(int i=0;i<vd2.size();i++)
	   cout<<vd2[i]<<" ";
	 cout<<endl;
	 if(isnan(vd2[1])||isnan(vd2[0]||isnan(vd2[2]))) 
	 {
		 cout<<"the mismatch factor is nan\n";
		 int k;
		 cin>>k;
	 }
   for(int i=0;i<5;i=i+2)
	 {
		 FF_Tmp=beamf[i]; if(beamo[i]<beamf[i]) FF_Tmp=beamo[i];
		 FF_Tmp=(beamo[i]-beamf[i])/FF_Tmp;
		 //FF[i+1]=FF_Tmp;
		 FF[i]=FF_Tmp;
		 FF_Tmp=beamo[i+1]-beamf[i+1];
		 if(fabs(beamf[i+1])>1.0) FF_Tmp/=beamf[i+1];
		 //FF[i]=FF_Tmp;
		 FF[i+1]=FF_Tmp;
	 }
//	 vvdFF.push_back(FF);
	 return dMax;
}
//int CTrackMatrix::Match_InSig(const vector<vector<double > >& vvd,int iStartCell)
////用beami
int CTrackMatrix::Match_Follow(const vector<vector<double > >& vvd,int iStartCell,int mt)
{
	 vector<double> vd;
	 double dEx,dEy,dEz;
	 int JF=0;
	 double dBG1,dBG2;
	 double dB1,dB2;
	 double dG1,dG2;
	 if(beami[0]<=0||beami[2]<=0||beami[4]<=0)
	   return 1;
		 beamf=beami;
	 if(mt!=8)
	 {
		 dG1=vvdStruct[iStartCell][8]/dErest+1.0;         //gama
		 dG2=vvdStruct[iStartCell+Period][8]/dErest+1.0;  //gama
		 dB1=sqrt(1.0-1.0/dG1/dG1);   //beta
		 dB2=sqrt(1.0-1.0/dG2/dG2);   //beta
		 dBG1=dB1*dG1;
		 dBG2=dB2*dG2;
		 beamf[0]*=(dBG2/dBG1);
		 beamf[2]*=(dBG2/dBG1);
		 beamf[4]*=sqrt(dB2*dB2*dB2/dB1/dB1/dB1*dG1/dG2);
	 }
	 //beamf[4]*=sqrt(dBG2*dBG2/dBG1/dBG1*dBG2/dBG1);
	 //beamf[4]*=sqrt(dBG2/dBG1);
	 //beamf[4]*=sqrt(dBG2/dBG1)*dB1*dB1/dB2/dB2;

	 // 产生随即分布的twiss参数
	 hepmatrixSig=vhdSig[iStartCell];
	 dEx=sqrt(hepmatrixSig[0][0]*hepmatrixSig[1][1]-pow(hepmatrixSig[1][0],2));
	 dEy=sqrt(hepmatrixSig[2][2]*hepmatrixSig[3][3]-pow(hepmatrixSig[3][2],2));
	 dEz=sqrt(hepmatrixSig[4][4]*hepmatrixSig[5][5]-pow(hepmatrixSig[5][4],2));
	 double dGx,dGy,dGz;
   dGx=(1.0+beami[1]*beami[1])/beami[0];
   dGy=(1.0+beami[3]*beami[3])/beami[2];
   dGz=(1.0+beami[5]*beami[5])/beami[4];
	 hepmatrixSig[0][0]=(beami[0]*dEx); //betax
	 hepmatrixSig[1][1]=(dGx*dEx);      //gamax
	 hepmatrixSig[1][0]=(-beami[1]*dEx); //alphx

	 hepmatrixSig[2][2]=(beami[2]*dEy);
	 hepmatrixSig[3][2]=(-beami[3]*dEy);
	 hepmatrixSig[3][3]=(dGy*dEy);

	 hepmatrixSig[4][4]=(beami[4]*dEz);
	 hepmatrixSig[5][4]=(-beami[5]*dEz);
	 hepmatrixSig[5][5]=(dGz*dEz);
	 CLHEP::HepMatrix vm=CLHEP::HepMatrix(6,6,1); // store the matrix multiply till the current cell
	 //跟踪一个周期
	 for(int i=0;i<Period;i++)
	 {
		 vd=vvdStruct[i+iStartCell];
		 DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		 vhdSig[iStartCell+i+1]=hepmatrixSig;
		 vm=hepmatrix*vm;
	 }             

	 dEx=sqrt(hepmatrixSig[0][0]*hepmatrixSig[1][1]-pow(hepmatrixSig[1][0],2));
	 dEy=sqrt(hepmatrixSig[2][2]*hepmatrixSig[3][3]-pow(hepmatrixSig[3][2],2));
	 dEz=sqrt(hepmatrixSig[4][4]*hepmatrixSig[5][5]-pow(hepmatrixSig[5][4],2));
	 beamo[0]=hepmatrixSig[0][0]/dEx;
	 beamo[2]=hepmatrixSig[2][2]/dEy;
	 beamo[4]=hepmatrixSig[4][4]/dEz;
	 beamo[1]=-hepmatrixSig[1][0]/dEx;
	 beamo[3]=-hepmatrixSig[3][2]/dEy;
	 beamo[5]=-hepmatrixSig[5][4]/dEz;
	 if(beamo[0]<=0||beamo[2]<=0||beamo[4]<=0)
		 return 1;
	 //beami修正为beamf
//	 cout<<"beamo is "<<endl;
//	 for(int i=0;i<6;i++)
//	 cout<<beamo[i]<<" ";
//	 cout<<endl;
	 return 0;

}
double CTrackMatrix::Match8_1(int nit,int iStart,int nc)
{
   int nm=4;double dd=1.0e-3;
	 int mm=nit;
	 int flag=1;
	 while(flag==1)
	 {
/* ---- DD IS THE CONVERGENCE CRITERION */
/* ---- MM IS THE MAXIMUM NUMBER OF ITERATIONS */
/* ---- FC IS THE MINIMUM VALUE OF THE FUNCTION FOUND SO FAR */
/* ---- NM IS THE NUMBER OF EQUATION.=NC */
		 nls_(&nm,&dd,&mm,iStart);
		 cout<<"input flag for nls_,0 end;1 continue"<<endl;
		 cin>>flag;
	 }
}
int CTrackMatrix::nls_(int*nm, double*dd, int*mm,int iStart)
{
    /* System generated locals */
		/* 函数会跟踪从iStart开始的一个周期，来得到匹配和失配参数,
		 * 但是变量的个数是nv个，需要匹配的参数个数是nc
		 */
    integer i__1, i__2, i__3;

    /* Local variables */
    static doublereal a[42]	/* was [6][7] */, c__[56]	/* was [7][8] */, d__;
    static integer i__, j, k, m, n;
    static doublereal s, y[42]	/* was [6][7] */, d1;
    static integer n1;
    static doublereal r1, r2, ai[36]	/* was [6][6] */, ff[7], pi_nls;
    static integer nn, it;
    static doublereal det, dmin__, dmax__;
//    extern /* Subroutine */ int func_(void);
    static integer kmin;
    static integer kmax, nerr, iseed;
    static doublereal useed;
		static integer c__7 = 7;
		static integer c__1 = 1;
//    extern /* Subroutine */ int matin1_(doublereal *, integer *, integer *, 
//	    integer *, integer *, integer *, doublereal *);
    static integer indexx[7];
    extern doublereal dustar_(integer *);
		//new add
    doublereal val[6], x[6], dFc, f[6];
    integer jf, nit, ijm[12]	/* was [2][6] */  ;
		vector<double> FF,vd3;
		extern void Convert_vd_array(vector<double>,doublereal[]);
		extern vector<double> Convert_array_vd(double *,int);

/* ---- NONLINEAR SYSTEM SOLVER USING REGULA FALSI TECHNIQUE. */
/* ---- NM IS THE NUMBER OF EQUATIONS */
/* ---- X IS THE INITIAL GUESS FOR THE SOLUTION VECTOR */
/*------iStart IS THE START NUMBER OF CELL*/
/* ---- DD IS THE CONVERGENCE CRITERION */
/* ---- MM IS THE MAXIMUM NUMBER OF ITERATIONS */
/* ---- FC IS THE MINIMUM VALUE OF THE FUNCTION FOUND SO FAR */
/* ---- IS IS SET TO 0, 1, OR -1, MEANING */
/* ---- CONVERGENCE, NO CONVERGENCE, OR STOP, RESPECTIVELY */
/* ---- SEARCHING CONTINUES UNTIL FX + D, OR M IS EXCEEDED. */
/* ---- CALLED BY MATCH;  CALLS FUNC, MATIN1 */
    n = *nm;
    d__ = *dd;
    m = *mm;

/* ---- SET SEED FOR RANDOM NUMBERS */

		iseed = 305;
		useed = dustar_(&iseed);
		srand( (unsigned)time( NULL ) );   
		/* L9: */
		if (nv < 1) {
			goto L70;
		}
		if (n < 1) {
			goto L70;
		}
		if (n > 6) {
			goto L70;            
		}
		n1 = nv + 1;
		i__1 = nv;
		for(int i=0;i<nv;i++)
		{
			x[i]=vvdStruct[iStart+i][7];
			//the condition should be changed if the fitting not only happend inside a period 
			//so does in L70
			//if(i<Period-1)  
			//{
			//	vvdStruct[iStart+i][6]=4.0;
			//	vvdStruct[iStart+i+1][4]=0.0;
			//}
		}
		jf=Match_Follow(vvdStruct,iStart,8);
		dFc=Match_MMF(vd3,FF,8);
		Convert_vd_array(FF,f);
		if(dFc<d__) goto L70;
		for (i__ = 1; i__ <= i__1; ++i__) {
			/* L10: */
			y[i__ - 1] = x[i__ - 1];
		}
		i__1 = n;
		for (i__ = 1; i__ <= i__1; ++i__) {
			/* L20: */
			//a[i__ - 1] = mcon_1.f[i__ - 1];
			a[i__ - 1] = f[i__ - 1];
		}
		kmin = 1;
		ff[0] = dFc;
		i__1 = n1;
		for (j = 2; j <= i__1; ++j) {
			i__2 = nv;
			for (i__ = 1; i__ <= i__2; ++i__) {
				r1 = duni_();
				r2 = duni_();
				//r1=double(rand()%100000)/100000.0;
				//r2=double(rand()%100000)/100000.0;
				pi_nls = r1 * .5f * (.5f - r2);
				y[i__ + j * 6 - 7] = (pi_nls + 1.f) * y[i__ - 1];
				/* L30: */
			}
		}
		it = 0;
		j = 2;
L40:
		i__2 = nv;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L50: */
			x[i__ - 1] = y[i__ + j * 6 - 7];
		}
		//func_();
		for(i__=0;i__<nv;i__++)
		{
			vvdStruct[iStart+i__][7]=x[i__];
			vvdStruct[iStart+i__+1][5]=x[i__];
		}
		jf=Match_Follow(vvdStruct,iStart,8);
		dFc=Match_MMF(vd3,FF,8);
		Convert_vd_array(FF,f);

		/* L60: */
		ff[j - 1] = dFc;
		if (dFc > d__) {
			goto L80;
		}
L70:
		for(int i=0;i<nv;i++)
		{
			vvdStruct[iStart+i][7]=x[i];
			vvdStruct[iStart+i+1][5]=x[i];
			//if(i<Period) 
			//{
			//	vvdStruct[iStart+i][6]=2.0;
			//	vvdStruct[iStart+i+1][4]=2.0;
			//}
		}
    cout<<vd3[0]<<" "<<vd3[1]<<" "<<vd3[2]<<endl;
    cout<<x[0]<<" "<<x[1]<<" "<<x[2]<<" "<<x[3]<<endl;
		return 0;
L80:
		i__2 = n;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L90: */
			a[i__ + j * 6 - 7] = f[i__ - 1];
		}
		if (jf == 0) {
			goto L110;
		}
		/* ---- GENERATE ANOTHER GUESS */
		i__2 = nv;
		for (i__ = 1; i__ <= i__2; ++i__) {
			r1 = duni_();
			r2 = duni_();
			//r1=double(rand()%100000)/100000.0;
			//r2=double(rand()%100000)/100000.0;
			pi_nls = r1 * (.5f - r2);
			y[i__ + j * 6 - 7] = (pi_nls + 1.f) * y[i__ - 1];
			/* L100: */
		}
		goto L40;
L110:
		++j;
		if (j <= n1) {
			goto L40;
		}
		/* ---- FIND BEST AND WORST SOLUTIONS */
L120:
		dmin__ = ff[0];
		dmax__ = dmin__;
		kmin = 1;
		kmax = 1;
		i__2 = n1;
		for (j = 2; j <= i__2; ++j) {
			d1 = ff[j - 1];
			if (d1 >= dmin__) {
				goto L130;
			}
			dmin__ = d1;
			kmin = j;
			goto L140;
L130:
			if (d1 <= dmax__) {
				goto L140;
			}
			dmax__ = d1;
			kmax = j;
L140:
			;
		}
		/* ---- CHECK FOR COMPLETION */
		/* L150: */
		i__2 = n;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L155: */
			f[i__ - 1] = a[i__ + kmin * 6 - 7];
		}
		i__2 = nv;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L160: */
			x[i__ - 1] = y[i__ + kmin * 6 - 7];
			vvdStruct[iStart+i__-1][7]=x[i__-1];
			vvdStruct[iStart+i__][5]=x[i__-1];
		}
		dFc = ff[kmin - 1];
		if (dmin__ <= d__) {
			goto L70;
		}
		if (it < m) {
			goto L165;
		}
//		func_();
		for(i__=0;i__<nv;i__++)
		{
			vvdStruct[iStart+i__][7]=x[i__];
			vvdStruct[iStart+i__+1][5]=x[i__];
		}
		cout<<endl;
		jf=Match_Follow(vvdStruct,iStart,8);
		dFc=Match_MMF(vd3,FF,8);
		//f=Convert_vd_array(FF);
		Convert_vd_array(FF,f);
		goto L70;
		/* ---- KEEP TRYING */
L165:
		++it;
		nn = n1 + 1;
		if (nv != n) {
			goto L250;
		}
		i__2 = n1;
		for (j = 1; j <= i__2; ++j) {
			c__[j * 7 - 7] = 1.f;
			c__[j + nn * 7 - 8] = 0.f;
			i__1 = n;
			for (i__ = 1; i__ <= i__1; ++i__) {
				/* L170: */
				c__[i__ + 1 + j * 7 - 8] = a[i__ + j * 6 - 7];
			}
		}
		c__[nn * 7 - 7] = 1.f;
    cout<<"the function a is "<<endl;
		for(i__=0;i__<42;i__++)
		{
			cout<<a[i__]<<" ";
			if((i__+1)%6==0) cout<<endl;
		}
    cout<<"the function c is "<<endl;
		for(i__=0;i__<56;i__++)
		{
			cout<<c__[i__]<<" ";
			if((i__+1)%7==0) cout<<endl;
		}

		matin1_(c__, &c__7, &n1, &c__1, indexx, &nerr, &det);
    cout<<"the function c after matin is "<<endl;
		for(i__=0;i__<56;i__++)
		{
			cout<<c__[i__]<<" ";
			if((i__+1)%7==0) cout<<endl;
		}
		if (nerr < 0) {
			goto L210;
		}
L175:
		i__1 = nv;
		for (i__ = 1; i__ <= i__1; ++i__) {
			x[i__ - 1] = 0.f;
			i__2 = n1;
			for (j = 1; j <= i__2; ++j) {
				/* L180: */
				x[i__ - 1] += c__[j + nn * 7 - 8] * y[i__ + j * 6 - 7];
			}
		}
L190:
		//func_();
		for(i__=0;i__<nv;i__++)
		{
			vvdStruct[iStart+i__][7]=x[i__];
			vvdStruct[iStart+i__+1][5]=x[i__];
		}
		jf=Match_Follow(vvdStruct,iStart,8);
		dFc=Match_MMF(vd3,FF,8);
		//f=Convert_vd_array(FF);
		Convert_vd_array(FF,f);
		/* L200: */
		if (jf < 0) {
			goto L120;
		} else if (jf == 0) {
			goto L230;
		} else {
			goto L210;
		}
L210:
		++it;
		if (it >= m) {
			goto L120;
		}
		i__2 = nv;
		for (i__ = 1; i__ <= i__2; ++i__) {
			r1 = duni_();
			r2 = duni_();
			//r1=double(rand()%100000)/100000.0;
			//r2=double(rand()%100000)/100000.0;
			pi_nls = r1 * .2f * (.5f - r2);
			x[i__ - 1] = (pi_nls + 1.f) * y[i__ + kmin * 6 - 7];
			/* L220: */
		}
		goto L190;
L230:
		i__2 = n;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L235: */
			a[i__ + kmax * 6 - 7] = f[i__ - 1];
		}
		i__2 = nv;
		for (i__ = 1; i__ <= i__2; ++i__) {
			/* L240: */
			y[i__ + kmax * 6 - 7] = x[i__ - 1];
		}
		ff[kmax - 1] = dFc;
		goto L120;
		/* ---- FEWER VARIABLES THAN CONDITIONS. DO SOMETHING FANCY. */
L250:
		i__2 = nv;
		for (j = 1; j <= i__2; ++j) {
			i__1 = n;
			for (i__ = 1; i__ <= i__1; ++i__) {
				/* L260: */
				ai[i__ + j * 6 - 7] = a[i__ + j * 6 - 7] - a[i__ + n1 * 6 - 7];
			}
		}
		i__1 = nv;
		for (i__ = 1; i__ <= i__1; ++i__) {
			i__2 = nv;
			for (j = 1; j <= i__2; ++j) {
				s = 0.f;
				i__3 = n;
				for (k = 1; k <= i__3; ++k) {
					/* L265: */
					s += ai[k + i__ * 6 - 7] * ai[k + j * 6 - 7];
				}
				/* L270: */
				c__[i__ + j * 7 - 8] = s;
			}
			s = 0.f;
			i__2 = n;
			for (k = 1; k <= i__2; ++k) {
				/* L280: */
				s += ai[k + i__ * 6 - 7] * a[k + n1 * 6 - 7];
			}
			c__[i__ + n1 * 7 - 8] = -s;
			/* L290: */
		}
		matin1_(c__, &c__7, &nv, &c__1, indexx, &nerr, &det);
		if (nerr < 0) {
			goto L210;
		}
		s = 0.f;
		i__1 = nv;
		for (i__ = 1; i__ <= i__1; ++i__) {
			/* L300: */
			s += c__[i__ + n1 * 7 - 8];
		}
		c__[n1 + n1 * 7 - 8] = 1.f - s;
		nn = n1;
		goto L175;
} /* nls_ */

double CTrackMatrix::Match8(int nit,int iStart,int nc)
{
	//corresponding to the match type 8 in trace-3d
	//beami and beamf is known
	
	//variable definition
  setDelt(0.0005);
	double dFc;
	vector<vector<double> > vvdRand;
	vector<vector<double> > vvdMMF; //存储nv+1组是配参数
	vector<double> vdVariable;
	vector<double> vd3;//储存三个方向的失配参数
	vector<double> FF;//临时传递六个方向的变换
	vector<double> vdFc;//store the biggest mismatch factor in three direction
	double dFcBest,dFcWorst;
	int    iFcBest,iFcWorst;
	double dY,dR1,dR2;   //tmp variable for rand distribution
	double JF=0;         //if beamf isn't reasonable,beamf=1
	double Conver_Fac=0;   //convergency factor. if(Conver_Fac<0 means no convergency)
	double d_Factor=0.5;   //the modulation function for rand distribution
	//capacity
	vdVariable.resize(nc);
	vvdRand.resize(nv+1);
	vdFc.resize(nv+1);
	vvdMMF.resize(nv+1);
	vvdFF.resize(nv+1);
	//store initinal value
	JF=Match_Follow(vvdStruct,iPeriod*Period);
	if(JF==1)  {cout<<"wrong beami\n";return 0;}
	dFc=Match_MMF(vd3,FF,8);
	if(dFc<Delt) return dFc;
	//if(dFc<Delt) goto L1000;
	for(int i=0;i<nv;i++)
		vdVariable[i]=vvdStruct[iStart+i][7];
	vdFc[0]=dFc;
	vvdMMF[0]=vd3;
	vvdFF[0]=FF;
	vvdRand[0]=vdVariable;
	dFcBest=dFc;dFcWorst=dFc;
	iFcBest=0;  iFcWorst=0;
	//begin fit
	srand( (unsigned)time( NULL ) );   
	//prepara for the matin1
	doublereal* p2DimRndDist = new doublereal[8*7];
	integer     dim1=7;
	integer     n1=nv+1;
	integer     n2=1;
	integer*    indexx= new integer[7];//right??
	integer     nerror;
	doublereal  determ;
	double      s;
	//recycle parameters. preclaimed because of the goto command
	int         i__,j__,k__,l;
	for(i__=0;i__<7*8;i__++)  p2DimRndDist[i__]=0;

	for(l=0;l<nit;l++)
	{
		//fit--generate nv groups of rand distribution
		for(j__=1;j__<nv+1;)   //每次产生维度＋1组
		{
			for(i__=0;i__<nv;i__++)
			{
				dR1=double(rand()%100000)/100000.0;
				dR2=double(rand()%100000)/100000.0;
				dY=d_Factor*dR1*(0.5-dR2);
				vdVariable[i__]=vvdRand[0][i__]*(1.0+dY);
				vvdStruct[iStart+i__][7]=vdVariable[i__];
			}
			JF=Match_Follow(vvdStruct,iStart);
			if(JF==0)
			{
				vvdRand[j__]=vdVariable;
				dFc=Match_MMF(vd3,FF,8);
				vvdFF[j__]=FF;
				vvdMMF[j__]=vd3;
				vdFc[j__]=dFc;
				if(dFc<Delt)  goto L1000;
				j__++;       
			}
			else
				d_Factor=1.0;
		}
		cout<<"the vvdRand is\n";
		for(i__=0;i__<vvdRand.size();i__++)
		{
			for(j__=0;j__<vvdRand[i__].size();j__++)
				cout<<vvdRand[i__][j__]<<" ";
			cout<<endl;
		}

L120:
		//find the best and worst mismatch factor
		dFcBest =vdFc[0];iFcBest=0;
		dFcWorst=vdFc[0];iFcWorst=0;
		for(j__=0;j__<vdFc.size();j__++)
		{
			dFc=vdFc[j__];
			if(dFc<dFcBest)  {dFcBest=dFc;iFcBest=j__;}
			if(dFc>dFcWorst) {dFcWorst=dFc;iFcWorst=j__;}
		}
		vdVariable=vvdRand[iFcBest];
		for(i__=0;i__<nv;i__++)
			vvdStruct[iStart+i__][7]=vdVariable[i__];
		FF=vvdFF[iFcBest];
		dFc=vdFc[iFcBest];
		if(dFc<Delt)  goto L1000;
		if(l>=nit-1)
		{
			for(i__=0;i__<nv;i__++)
			{
				vvdStruct[i__][7]=vdVariable[i__];
				cout<<vdVariable[i__]<<" ";
			}
			cout<<endl;
			JF=Match_Follow(vvdStruct,iStart);
			dFc=Match_MMF(vd3,FF,8);
			cout<<"l and dFc is "<<l<<" "<<dFc<<endl;
			goto L1000;
		}
		cout<<"l and dFc is "<<l<<" "<<dFc<<endl;
L165:
		++l;
		if(nv!=nc)  goto L250;
		cout<<"begin vvdFF\n" ;
		for(i__=0;i__<nv+1;i__++)
		{
			for(j__=0;j__<nv;j__++)
				cout<<vvdFF[i__][j__]<<" ";
			cout<<endl;
		}
		//matin function form trace3d,determines the convergency direction 
		//for(i__=0;i__<(nv+1)*(nv+2);i__++)
		//{
		//	if(i__%(nv+1)==0)
		//		p2DimRndDist[i__]=1;
		//	else if(i__<(nv+1)*(nv+1))
		//		p2DimRndDist[i__]=vvdFF[i__/(nv+1)][i__%(nv+1)-1];
		//	else 
		//		p2DimRndDist[i__]=0;
		//}
		for(i__=0;i__<nv+2;i__++)
		{
			p2DimRndDist[i__*7]=1;
			for(j__=0;j__<nc;j__++)
			{
			  if(i__<nv+1)
				p2DimRndDist[i__*7+j__+1]=vvdFF[i__][j__];
			}
		}
		cout<<"begin p2DimRndDist \n";
		for(i__=0;i__<56;i__++)
		{
			cout<<p2DimRndDist[i__]<<" ";
			if((i__+1)%7==0) cout<<endl;
		}
		matin1_(p2DimRndDist, &dim1, &n1,&n2,indexx,&nerror,&determ); 
		cout<<"p2DimRndDist \n";
		for(i__=0;i__<56;i__++)
		{
			cout<<p2DimRndDist[i__]<<" ";
			if((i__+1)%7==0) cout<<endl;
		}
		Conver_Fac=nerror;
		//Conver_Fac<0 means no convergency,
		//optimize arrounds the current best; 
		if (Conver_Fac < 0) { goto L210; } 
L175:
		//if conver_fac=0, find the new result according to the conver_fac direction
		for(i__=0;i__<nv;i__++)
		{
			vdVariable[i__]=0;
			for(k__=0;k__<nv+1;k__++)  ///!!!
			{
				vdVariable[i__]+=(p2DimRndDist[k__+7*(nv+1)]*vvdRand[k__][i__]);
			}
			vvdStruct[iStart+i__][7]=vdVariable[i__];
		}
L190:
		//cal the new mismatch factor,if the beamf is reasonable and 
		//mmf is small enough and , then stop
		//else if the beamf isn't reasonable, then 
		JF=Match_Follow(vvdStruct,iStart);
		dFc=Match_MMF(vd3,FF,8);    //vd3 is mismatch factor in three dimension
		if(dFc<Delt) goto L1000;
		/* L200: */
		if(JF<0)  goto L120;
		else if(JF==0) goto L230;
		else  goto L210;
L210:
		l=l+1;
		if (l >= nit-1) { goto L120; }
		for(i__=0;i__<nv;i__++)
		{
			dR1=double(rand()%10000)/10000.0;
			dR2=double(rand()%10000)/10000.0;
			dY=0.2*dR1*(0.5-dR2);
			vdVariable[i__]=vvdRand[iFcBest][i__]*(1+dY);
		}
		goto L190;
L230:
		vvdRand[iFcWorst]=vdVariable;
		vvdFF[iFcWorst]=FF;
		vdFc[iFcWorst]=dFc;
		goto L120;
L250:
	//	//if the variables is less than the equations
	//	//copy from trace_c.c line 6121
	for(j__=0;j__<nv;j__++)
		for(i__=0;i__<nc;i__++)
			vvdFF[j__][i__]=vvdFF[j__][i__]-vvdFF[nv][i__];
	for (i__= 0; i__< nv; i__++) 
	{
		for (j__= 0; j__< nv; j__++) 
		{
			s = 0.f;
			for (k__= 0; k__< nc; k__++) 
			{
				/* L265: */
				s += vvdFF[i__][k__]*vvdFF[j__][k__];
			}
			/* L270: */
			//对角矩阵
			p2DimRndDist[i__+j__*nv]=s;
			//c__[i__ + j * 7 - 8] = s;
		}
		s = 0.f;
		for (k__= 0; k__< nc; k__++) 
		{
			/* L280: */
			s += vvdFF[i__][k__]*vvdFF[nv][k__];
		}
		p2DimRndDist[i__+(nv+1)*(nv+1)]=s;
		//c__[i__ + n1 * 7 - 8] = -s;
		/* L290: */
	}
	//matin1_(c__, &c__7, &nv, &c__1, indexx, &nerr, &det);
	dim1=nv+1;
	n1=nv+1;
	matin1_(p2DimRndDist, &dim1, &n1,&n2,indexx,&nerror,&determ); 
	if (Conver_Fac < 0) { goto L210; }
	s = 0.f;
	for (i__ = 0; i__ < nv; i__++) {
		/* L300: */
		s+=p2DimRndDist[i__+(nv+1)*(nv+1)];
		//s += c__[nv][i__];
	}
	p2DimRndDist[(nv+2)*(nv+1)-1]=s;
	//c__[n1 + n1 * 7 - 8] = 1.f - s;
		nc = nv+1;
	goto L175;
	}
L1000:
  for(i__=0;i__<nv;i__++)
	{
	  vvdStruct[iStart+i__][7]=vdVariable[i__];
		cout<<vdVariable[i__]<<" ";
	}
	cout<<endl;
	return dFc;
}
double CTrackMatrix::Match(int nit,int iStart)
{
//return 0 means complete the match process
//iStart means the start cell of matching section, 
//iStart is related to the match_follow() function 
  setDelt(0.000005);
	vector<double> vd;
	double dEx,dEy,dEz;

	static integer iseed;
	static doublereal useed;
	iseed = 305;
	useed = dustar_(&iseed);

	for(int i=0;i<beami.num_row();i++)
	{
		int k=i+1;
		if(i<2) k=i;
		else if(i>3) k=i+2;
		beami[i]=hepvectorTwiss[k];
	}
	srand( (unsigned)time( NULL ) );   
	//srand(305 );   
	double dR1,dR2,dY;
	int    Conver_Fac=0;
	vector<double> vdFc;
	double dFc;
	double dFcBest, dFcWorst;
	int    iFcBest,iFcWorst;
	int    JF=0;
	vector<vector<double> > vvdRand;
	vector<double> vdRand_Tmp;
	vector<vector<double> > vvdMMF;
	vector<double> vd3;//储存三个方向的失配参数
	vector<double> FF; //临时传递六个方向的变换
	vdFc.resize(nv+1);
	vvdMMF.resize(nv+1);
	vdRand_Tmp.resize(nv);
	vvdRand.resize(nv+1);
	vvdFF.resize(nv+1);
	for(int i=0;i<beami.num_row();i++)
		vdRand_Tmp[i]=beami[i];
	vvdRand[0]=vdRand_Tmp;
	Match_Follow(vvdStruct,iStart);
	dFc=Match_MMF(vd3,FF);
	vvdFF[0]=FF;
	vvdMMF[0]=vd3;
	vdFc[0]=dFc;
	double d_Factor=0.5;
	if(dFc<Delt) goto L1000;
	//if(dFc<Delt) return dFc;
	dFcBest=dFc;iFcBest=0;
	dFcWorst=dFc;iFcWorst=0;
	for(int l=0;l<nit;l++)
	{
		for(int j=1;j<nv+1;)   //每次产生维度＋1组
		{
			for(int i=0;i<beami.num_row();i++)
			{
				dR1=double(rand()%100000)/100000.0;
				dR2=double(rand()%100000)/100000.0;
				//dR1=duni_();
				//dR2=duni_();
				dY=d_Factor*dR1*(0.5-dR2);
				int k=i+1;
				if(i<2) k=i;
				else if(i>3) k=i+2;
				//beami[i]=hepvectorTwiss[k]*(1+dY);
				beami[i]=vvdRand[0][i]*(1.0+dY);
			}
			if(beami[0]>0&&beami[2]>0&&beami[4]>0)
			{
				JF=Match_Follow(vvdStruct,iStart);
				if(JF==0)
				{
					for(int i=0;i<beami.num_row();i++)
						vdRand_Tmp[i]=beami[i];
					vvdRand[j]=vdRand_Tmp;
					dFc=Match_MMF(vd3,FF);
					vvdFF[j]=FF;
					vvdMMF[j]=vd3;
					vdFc[j]=dFc;
					if(dFc<Delt)  goto L1000;
					//if(dFc<Delt)  return 0;
					j++;       
				}
				else
					d_Factor=1.0;
			}
			else 
				d_Factor=1.0;
		}
//		for(int i=0;i<vvdRand.size();i++)
//		{
//			for(int j=0;j<vvdRand[i].size();j++)
//				cout<<vvdRand[i][j]<<" ";
//			cout<<endl;
//		}
	/*
	JF=Match_Follow(vvdStruct);
	dFc=Match_MMF(vd3,FF);
	if(dFc<Delt)  goto L70;
	vdRand_Tmp.resize(0);
	vvdRand.resize(nv+1);
	for(int i=0;i<beami.num_row();i++)
	{
		vdRand_Tmp.push_back(beami[i]);
	}
	vvdFF[0]=FF;
	vvdRand[0]=vdRand_Tmp;
	vdFc[0]=dFc;
	iFcBest=0;dFcBest=dFc;
	vdRand_Tmp.resize(nv);
	for(int j=1;j<nv+1;)   //每次产生维度＋1组
	{
		for(int i=0;i<beami.num_row();i++)
		{
			dR1=double(rand()%100000)/100000.0;
			dR2=double(rand()%100000)/100000.0;
			dY=0.5*dR1*(0.5-dR2);
			int k=i+1;
			if(i<2) k=i;
			else if(i>3) k=i+2;
			//beami[i]=hepvectorTwiss[k]*(1+dY);
			beami[i]=vvdRand[0][i]*(1.0+dY);
			vdRand_Tmp[i]=beami[i];
		}
		vvdRand[j]=vdRand_Tmp;
	}
	int l=0;
	int n=1;
L40:
	for(int i=0;i<beami.num_row();i++)
		beami[i]=vvdRand[n][i];  //??
	JF=Match_Follow(vvdStruct);
	dFc=Match_MMF(vd3,FF);
	vdFc[n]=dFc;
	if(dFc>Delt)  goto L80;
L70:
  return 0;
L80:
	vvdFF[n]=FF;
	if(JF==0)  goto L110;

	for(int i=0;i<beami.num_row();i++)
	{
		dR1=double(rand()%100000)/100000.0;
		dR2=double(rand()%100000)/100000.0;
		dY=dR1*(0.5-dR2);
		int k=i+1;
		if(i<2) k=i;
		else if(i>3) k=i+2;
		//beami[i]=hepvectorTwiss[k]*(1+dY);
		beami[i]=vvdRand[0][i]*(1.0+dY);
		vdRand_Tmp[i]=beami[i];
	}
	vvdRand[n]=vdRand_Tmp;
	goto L40;
L110:
  ++n;
	if(n<=nv) goto  L40
	*/


/* ---- GENERATE ANOTHER GUESS */
L120:
      dFcBest =vdFc[0];iFcBest=0;
      dFcWorst=vdFc[0];iFcWorst=0;
			for(int j=0;j<vdFc.size();j++)
			{
			  dFc=vdFc[j];
				if(dFc<dFcBest) {dFcBest=dFc;iFcBest=j;}
				if(dFc>dFcWorst) {dFcWorst=dFc;iFcWorst=j;}
			}
			for(int j=0;j<beami.num_row();j++)
			{
			  beami[j]=vvdRand[iFcBest][j];
			}
			FF=vvdFF[iFcBest];
			dFc=vdFc[iFcBest];
			//cout<<"best twiss is ";
			//for(int i=0;i<beami.num_row();i++)
			//	cout<<beami[i]<<" ";
			//cout<<endl;
			//cout<<"wors twiss is ";
			//for(int i=0;i<beami.num_row();i++)
			//	cout<<vvdRand[iFcWorst][i]<<" ";
			//cout<<endl;
			//if(dFc<Delt)  return 0;
			if(dFc<Delt)  goto L1000;
			if(l>=nit-1)
			{
				JF=Match_Follow(vvdStruct,iStart);
				dFc=Match_MMF(vd3,FF);
				cout<<"l and dFc is "<<l<<" "<<dFc<<endl;
				goto L1000;
			}
			cout<<"l and dFc is "<<l<<" "<<dFc<<endl;
			++l;

			//int matin1_(doublereal *a, integer *dim1, integer *n1, 
			//integer *n2, integer *indexx, integer *nerror, doublereal *determ)
			doublereal* p2DimRndDist = new doublereal[56];
			integer     dim1=7;
			integer     n1=7;
			integer     n2=1;
			integer*    indexx= new integer[8];
			integer     nerror;
			doublereal  determ;
			int         tmp;
			for(int i=0;i<56;i++)
			{
				if(i%7==0)
					p2DimRndDist[i]=1;
				else if(i<49)
					p2DimRndDist[i]=vvdFF[i/7][i%7-1];
				else 
					p2DimRndDist[i]=0;
			}
			matin1_(p2DimRndDist, &dim1, &n1,&n2,indexx,&nerror,&determ); 
			//cout<<"after matin1"<<endl;
			//double sum_last_line=0;
			//for(int i=0;i<56;i++)
			//{
			//   cout<<p2DimRndDist[i]<<" ";
			//	 if((i+1)%7==0) cout<<endl;
			//}
			//cout<<"sum_last_line is "<<sum_last_line<<endl;
			Conver_Fac=nerror;
			if (Conver_Fac < 0) { goto L210; }
L175:
			for(int i=0;i<beami.num_row();i++)
			{
				beami[i]=0;
				for(int k=0;k<nv+1;k++)  ///!!!
				{
					beami[i]+=(p2DimRndDist[k+49]*vvdRand[k][i]);
				}
			}
L190:
			JF=Match_Follow(vvdStruct,iStart);
			dFc=Match_MMF(vd3,FF);    //vd3 is mismatch factor in three dimension
			/* L200: */
			if(JF<0)  goto L120;
			else if(JF==0) goto L230;
			else  goto L210;
L210:
  	  l=l+1;
			if (l >= nit-1) { goto L120; }
			for(int i=0;i<beami.num_row();i++)
			{
				dR1=double(rand()%10000)/10000.0;
				dR2=double(rand()%10000)/10000.0;
				//dR1=duni_();
				//dR2=duni_();
				dY=0.2*dR1*(0.5-dR2);
				beami[i]=vvdRand[iFcBest][i]*(1+dY);
			}
			goto L190;
L230:
			for(int i=0;i<beami.num_row();i++)
			{
				vvdRand[iFcWorst][i]=beami[i];
			}
			vvdFF[iFcWorst]=FF;
			vdFc[iFcWorst]=dFc;
			goto L120;

			/*
				 JF=1;
				 while(JF>0&&l<nit-1)
				 {
			//here is not right

				for(int i=0;i<beami.num_row();i++)
				{
					dR1=double(rand()%10000)/10000.0;
					dR2=double(rand()%10000)/10000.0;
					dY=0.5*dR1*(0.5-dR2);
					beami[i]=vvdRand[iFcBest][i]*(1+dY);
				}
				JF=Match_Follow(vvdStruct);
				dFc=Match_MMF(vd3,FF);    //vd3 is mismatch factor in three dimension
				l++;
			}
			if(Conver_Fac==0)
			{
				for(int i=0;i<beami.num_row();i++)
				{
					beami[i]=0;
					for(int k=0;k<nv+1;k++)  ///!!!
					{
						beami[i]+=(p2DimRndDist[k+49]*vvdRand[k][i]);
					}
				}
			}
			if(JF<0&&Conver_Fac==0)
			{; }
			else if(JF==0&&Conver_Fac==0)
			{
				dFc=Match_MMF(vd3,FF);    //vd3 is mismatch factor in three dimension
				for(int i=0;i<beami.num_row();i++)
				{
					vvdRand[iFcWorst][i]=beami[i];
					vvdFF[iFcWorst][i]=FF[i];
				}
				FF[iFcWorst]=dFc;
			}
			*/
	}
L1000:
//revise hepvectortwiss
	return dFc;
}
void CTrackMatrix::test()
{
   vector<double> vd;
//   Match(40,24);
	for(int i=0;i<beami.num_row();i++)
	{
		int k=i+1;
		if(i<2) k=i;
		else if(i>3) k=i+2;
		beami[i]=hepvectorTwiss[k];
	}
	beamf=beami;
		//beami={0.141701, 0.979198, 0.519549, -1.9441, 0.342303, -0.0248402);
		//beamo={0.141701, 0.979198, 0.519549, -1.9441, 0.342303, -0.0248402);
	nv=3;
	Match8_1(10,0,4);
	//Match8(10,0,4);
	//Match8_1(int nit,int iStart,int nc)
 
//0.1533 0.979189 0.562081 -1.9441 0.384764 -0.0248404 
//0.153301 0.979198 0.56208 -1.9441 0.384764 -0.0248402 
//	 ofstream of("rand_test");
//	 CLHEP::HepRandomEngine* theNewEngine=CLHEP::HepJamesRandom;
//	 CLHEP::HepRandom::setTheEngine(theNewEngine);
//	 CLHEP::HepRandom::setTheSeed(theNewEngine);
//	 double a=CLHEP::RandomFlat::shoot();
//	 cout<<"a is "<<a<<endl;
	 /*
	 double  a[20]={1,2.1,2,2.3,1,2,2.22,2.31,1,1.9,2.1,1.91,1,1.82,1.88,1.96,1,0,0,0};
	 doublereal* p2DimRndDist = new doublereal[20];
	 integer    dim1=4;
	 integer    n1=4;
	 integer    n2=1;
	 integer*    indexx= new integer[8];
	 integer    nerror;
	 doublereal    determ;
	 int         tmp;
	 cout<<"vvdFF before matin1"<<endl;
	 for(int i=0;i<20;i++)
	 {
	   p2DimRndDist[i]=a[i];
	 }
	 cout<<"before matin1"<<endl;
	 for(int i=0;i<20;i++)
	 {
		 cout<<p2DimRndDist[i]<<" ";
		 if((i+1)%4==0) cout<<endl;
	 }
	 matin1_(p2DimRndDist, &dim1, &n1,&n2,indexx,&nerror,&determ); 
	 cout<<"after matin1"<<endl;
	 for(int i=0;i<20;i++)
	 {
		 cout<<p2DimRndDist[i]<<" ";
		 if((i+1)%4==0) cout<<endl;
	 }
	 */
}
void CTrackMatrix::Equip_Cell(double dx,double dy,int _xFlag,int _yFlag)
{
  //nlopt::opt opt(nlopt::GN_ISRES, Period);  //x ok;y not works
  nlopt::opt opt(nlopt::LN_COBYLA, Period);  //x ok;y not works
	double step=0.1;
	//bound
  std::vector<double> lb(Period);
  std::vector<double> ub(Period);
  for(int i=0;i<lb.size();i++)
	{
		lb[i]=-4; ub[i]=6;
	}
  opt.set_lower_bounds(lb);
  opt.set_upper_bounds(ub);
  int xFlag=_xFlag;
	int yFlag=_yFlag;
  s_trackMatrix=this;
  //constraints
  my_constraint_data2 data[2] = { {0,dx}, {1,dy} };

	opt.add_inequality_constraint(myvconstraint2, &data[0],1.0/180.0*pi);
	opt.add_inequality_constraint(myvconstraint2, &data[1],1.0/180.0*pi);
  //objective
	if(xFlag==0)
		opt.set_min_objective(myvfunc3, NULL);
  //opt.set_min_objective(myvfunc2, NULL);
	else if(xFlag==1)
		opt.set_min_objective(myvfunc, NULL);
  //termination
	opt.set_maxeval(10000);  //works !!
	opt.set_maxtime(60);//max time
	opt.set_initial_step(2);
	opt.set_population(40);
	opt.set_xtol_abs(1.0e-8);
	opt.set_ftol_abs(1.0e-8);

  std::vector<double> x(Period);
	//initinal
	iCount=0;
	for(int i=0;i<x.size();i++)
		x[i] = xFlag*step+yFlag*step;
//	if(xFlag==0)
//	  {x[0]=2.0;x[1]=2.0;x[2]=2.0;x[3]=2.0;}
  double minf;
	nlopt::result result=opt.optimize(x, minf);
	cout<<result<<endl;
	if (result < 0) {
		printf("nlopt failed!\n");
	}
	else {
		printf("found minimum at f(%g,%g,%g,%g) = %0.10g\n", x[0], x[1],x[2],x[3],  minf);
	}
	vector<double> vd;
	double dQuad_tmp=0;
	for(int j=0;j<Period;j++)
	{
		vd=vvdStruct[iPeriod*Period+j];
		vd[7]=vd[7]+x[j];
		if(j>0)
			vd[5]=dQuad_tmp;
		vvdStruct[iPeriod*Period+j]=vd;
		dQuad_tmp=vd[7];
		DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		vhdSig[iPeriod*Period+j+1]=hepmatrixSig;
	}
	//if it is not the last period 
	if((iPeriod+1)*Period<vvdStruct.size()-1)
	{
		vd=vvdStruct[(iPeriod+1)*Period];
		vd[5]=dQuad_tmp;
		vvdStruct[(iPeriod+1)*Period]=vd;
	}
}
void CTrackMatrix::Equip()
{
  nlopt::opt opt(nlopt::GN_ISRES, Period);  //x ok;y not works

  std::vector<double> lb(Period);
  std::vector<double> ub(Period);
  for(int i=0;i<lb.size();i++)
	{lb[i] =-20.0; ub[i] =20.0; }
  opt.set_lower_bounds(lb);
  opt.set_upper_bounds(ub);

  s_trackMatrix=this;

  my_constraint_data data[2] = { {0}, {1} };

  opt.add_inequality_constraint(myvconstraint, &data[0],0.01);
  opt.add_inequality_constraint(myvconstraint, &data[1],0.01);

  opt.set_min_objective(myvfunc, NULL);

	opt.set_maxeval(10000);  //works !!
	opt.set_maxtime(60);//max time
	//opt.set_ftol_abs(100);//max time
	opt.set_initial_step(2);
	opt.set_population(40);

  std::vector<double> x(Period);
	vector<double> vd;
  double minf;
  int Per_Num=(s_trackMatrix->vvdStruct.size())/Period;
  ofstream of("Equip_Result");
	vhdSig_Equip.resize(s_trackMatrix->vhdSig.size());
	vhdSig_Equip[0]=s_trackMatrix->vhdSig[0];
	//vector<vector<double> > vvdEquip;
	vector<double> vdP_ATmp;
  for(iPeriod=0;iPeriod<Per_Num;iPeriod++)
  {
	  iCount=0;
    for(int i=0;i<x.size();i++)
      x[i] = 0;
		s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];
    nlopt::result result=opt.optimize(x, minf);
		cout<<result<<endl;
		if (result < 0) {
			printf("nlopt failed!\n");
		}
		else {
			printf("found minimum at f(%g,%g,%g,%g) = %0.10g\n", x[0], x[1],x[2],x[3],  minf);
		}
		for(int i=0;i<x.size();i++)
		{
			s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];
			vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
//			vd[7]+=x[i];
			s_trackMatrix->vvdStruct[iPeriod*x.size()+i]=vd;
			s_trackMatrix->DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
			vhdSig_Equip[iPeriod*x.size()+i+1]=s_trackMatrix->hepmatrixSig;
			vdP_ATmp=s_trackMatrix->CalSig(vhdSig_Equip,iPeriod*x.size()+i);
			of<<iPeriod*x.size()+i<<" "<<x[i]<<" ";
			for(int j=0;j<vdP_ATmp.size();j++)
      {
				of<<vdP_ATmp[j]<<" ";
			}
			of<<endl;
		}
	  cout<<"iPeriod is "<<iPeriod<<endl;
  }
	of.close();
}
double myvfunc(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data)
{
  double sum=0;
  for(int i=0;i<x.size();i++)
		sum+=x[i];
		//sum+=x[i]*x[i];
  return fabs(sum);
}
double myvfunc3(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data)
{
	vector<double> FF,vdMisMatch,vd;
	double dFc;
	s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];
	double          dQuad_tmp;
  for(int i=0;i<x.size();i++)
  {
    vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
    vd[7]+=x[i];
		if(i>0)
			vd[5]=dQuad_tmp;
		s_trackMatrix->DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		//s_trackMatrix->vhdSig[iPeriod*x.size()+i+1]=s_trackMatrix->hepmatrixSig;
		dQuad_tmp=vd[7];
  }
	//cal beamf according to beami
	s_trackMatrix->beamf=s_trackMatrix->beami;
	double dBG1,dBG2;
	double dB1,dB2;
	double dG1,dG2;
	dG1=s_trackMatrix->vvdStruct[iPeriod*x.size()][8]/dErest+1.0;         //gama
	dG2=s_trackMatrix->vvdStruct[(iPeriod+1)*x.size()][8]/dErest+1.0;  //gama
	dB1=sqrt(1.0-1.0/dG1/dG1);   //beta
	dB2=sqrt(1.0-1.0/dG2/dG2);   //beta
	dBG1=dB1*dG1;
	dBG2=dB2*dG2;
	s_trackMatrix->beamf[0]*=(dBG2/dBG1);
	s_trackMatrix->beamf[2]*=(dBG2/dBG1);
	s_trackMatrix->beamf[4]*=sqrt(dB2*dB2*dB2/dB1/dB1/dB1*dG1/dG2);
	//cal beamo according to hepmatrixSig
	double dEx,dEy,dEz;
	dEx=sqrt(s_trackMatrix->hepmatrixSig[0][0]*s_trackMatrix->hepmatrixSig[1][1]-pow(s_trackMatrix->hepmatrixSig[1][0],2));
	dEy=sqrt(s_trackMatrix->hepmatrixSig[2][2]*s_trackMatrix->hepmatrixSig[3][3]-pow(s_trackMatrix->hepmatrixSig[3][2],2));
	dEz=sqrt(s_trackMatrix->hepmatrixSig[4][4]*s_trackMatrix->hepmatrixSig[5][5]-pow(s_trackMatrix->hepmatrixSig[5][4],2));
	s_trackMatrix->beamo[0]= s_trackMatrix->hepmatrixSig[0][0]/dEx;
	s_trackMatrix->beamo[2]= s_trackMatrix->hepmatrixSig[2][2]/dEy;
	s_trackMatrix->beamo[4]= s_trackMatrix->hepmatrixSig[4][4]/dEz;
	s_trackMatrix->beamo[1]=-s_trackMatrix->hepmatrixSig[1][0]/dEx;
	s_trackMatrix->beamo[3]=-s_trackMatrix->hepmatrixSig[3][2]/dEy;
	s_trackMatrix->beamo[5]=-s_trackMatrix->hepmatrixSig[5][4]/dEz;
	
	//cal maximum mismatch factor
	dFc=s_trackMatrix->Match_MMF(vdMisMatch,FF);
	cout<<" dFc is "<<dFc<<" ";
	return dFc;
}
double myvfunc2(const std::vector<double> &x, std::vector<double> &grad, void *my_func_data)
{
  //cal the mismatch factor
	vector<double> FF,vdMisMatch,vd;
	double dFc;
	s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];
	vector<double> vdQuad_tmp;
	double          dQuad_tmp;
	vdQuad_tmp.resize(x.size());
  for(int i=0;i<x.size();i++)
  {
    vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
    vd[7]+=x[i];
		vdQuad_tmp[i]=vd[7];
		if(i>0)
			vd[5]=dQuad_tmp;
    s_trackMatrix->vvdStruct[iPeriod*x.size()+i]=vd;
		dQuad_tmp=vd[7];
  }
	//cal beamo according to hepmatrixSig,beamf=beami;
	s_trackMatrix->Match_Follow(s_trackMatrix->vvdStruct,iPeriod*x.size());
	dFc=s_trackMatrix->Match_MMF(vdMisMatch,FF);
  for(int i=0;i<x.size();i++)
  {
    vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
		vd[7]=vdQuad_tmp[i];
		if(i>0)
			vd[5]=vdQuad_tmp[i-1];
    s_trackMatrix->vvdStruct[iPeriod*x.size()+i]=vd;
		for(int j=0;j<vd.size();j++)
			cout<<vd[j]<<" ";
		cout<<endl;
  }
	cout<<"the dFc is  "<<dFc<<" ";
	return dFc;
}
double myvconstraint(const std::vector<double> &x, std::vector<double> &grad, void *data)
{   
  iCount++;
  my_constraint_data *d = reinterpret_cast<my_constraint_data*>(data);
//a=0 or 1    a=0 means x,a=1 means y
  int Direction=d->a;
  std::vector<double> vd;
  double ph_adv=0;
	vector<double> vdP_ATmp;
	//s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[0];//!!!!
	s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];//!!!!
  for(int i=0;i<x.size();i++)
  {
    vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
    vd[7]+=x[i];
		s_trackMatrix->DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		//vhdSig_Equip.push_back(s_trackMatrix->hepmatrixSig);
		vhdSig_Equip[iPeriod*x.size()+i+1]=s_trackMatrix->hepmatrixSig;
		vdP_ATmp=s_trackMatrix->CalSig(vhdSig_Equip,iPeriod*x.size()+i);

    ph_adv+=(vdP_ATmp[Direction]);
  }
//    cout<<iCount<<endl;
//	cout<<" constraint is "<<ph_adv-s_trackMatrix->vvdPh_Av_Equip[iPeriod][Direction]<<" "
//			<<x[0]<<" "<<x[1]<<" "<<x[2]<<" "<<x[3]<<endl;
  return fabs(ph_adv-s_trackMatrix->vvdPh_Av_Equip[iPeriod][Direction])-0.8;
}
double myvconstraint2(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
  iCount++;
  my_constraint_data2 *d = reinterpret_cast<my_constraint_data2*>(data);
//a=0 or 1    a=0 means x,a=1 means y
  int Direction=d->a;
	double  value=d->b;
  std::vector<double> vd;
  double ph_adv=0;
	vector<double> vdP_ATmp;
	s_trackMatrix->hepmatrixSig=s_trackMatrix->vhdSig[iPeriod*x.size()];//!!!!
	CLHEP::HepMatrix  hmTmp=CLHEP::HepMatrix(6,6,1);
	double dQuad_tmp=0;
  for(int i=0;i<x.size();i++)
  {
    vd=s_trackMatrix->vvdStruct[iPeriod*x.size()+i];
    vd[7]+=x[i];
		if(i>0)
			vd[5]=dQuad_tmp;
		s_trackMatrix->DtlCell(vd[0],vd[1],vd[2],vd[3],vd[4],vd[5],vd[6],vd[7],vd[8]);
		//vhdSig_Equip.push_back(s_trackMatrix->hepmatrixSig);
		s_trackMatrix->vhdSig[iPeriod*x.size()+i+1]=s_trackMatrix->hepmatrixSig;
		hmTmp=s_trackMatrix->hepmatrix*hmTmp;
		dQuad_tmp=vd[7];
  }
	//vdP_ATmp=s_trackMatrix->CalSig2(hmTmp);
	vdP_ATmp=s_trackMatrix->CalSig3(s_trackMatrix->vhdSig,hmTmp,iPeriod*x.size());
	ph_adv=vdP_ATmp[Direction];
	cout<<"iCount is "<<iCount<<" ";
	if(Direction==0)
	cout<<"x"<<" ";
	else if(Direction==1)
	cout<<"y"<<" ";
	cout<<"diff-constraint "<<ph_adv-value<<endl;
  //return fabs(ph_adv-value)-2.0/180.0*pi;
  return fabs(ph_adv-value);
}
void CTrackMatrix::getStruct()
{
	ofstream of("Struct");
	for(int i=0;i<vvdStruct.size();i++)
	{
		for(int j=0;j<vvdStruct[i].size();j++)
			of<<vvdStruct[i][j]<<" ";
		of<<endl;
	}
}
